Guest User

ntd_mapping log

a guest
Oct 10th, 2019
302
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. method_type: 2
  2. use_odom: 0
  3. use_imu: 0
  4. imu_upside_down: 0
  5. imu_topic: imu_raw
  6. incremental_voxel_update: 0
  7. (tf_x,tf_y,tf_z,tf_roll,tf_pitch,tf_yaw): (0, 0, 0, 0, 0, 0)
  8. -----------------------------------------------------------------
  9. Sequence number: 4930
  10. Number of scan points: 6294 points.
  11. Number of filtered scan points: 722 points.
  12. transformed_scan_ptr: 6294 points.
  13. map: 6294 points.
  14. NDT has converged: 1
  15. Fitness score: 24.2114
  16. Number of iteration: 2
  17. (x,y,z,roll,pitch,yaw):
  18. (-0.00378692, 0.0164724, -0.000223981, -0.00047122, -0.00104701, -0.00415465)
  19. Transformation Matrix:
  20. 0.999991 0.00415513 -0.00104504 -0.00378692
  21. -0.00415463 0.999991 0.000475569 0.0164724
  22. 0.00104701 -0.000471219 0.999999 -0.000223981
  23. 0 0 0 1
  24. shift: 0.016902
  25. -----------------------------------------------------------------
  26. (Processed/Input): (1 / 6)
  27. -----------------------------------------------------------------
  28. Sequence number: 4931
  29. Number of scan points: 6294 points.
  30. Number of filtered scan points: 721 points.
  31. transformed_scan_ptr: 6294 points.
  32. map: 6294 points.
  33. NDT has converged: 1
  34. Fitness score: 24.2235
  35. Number of iteration: 2
  36. (x,y,z,roll,pitch,yaw):
  37. (-0.00761295, 0.0219133, 0.000314394, -0.000697163, -0.00104286, -0.00424208)
  38. Transformation Matrix:
  39. 0.99999 0.00424279 -0.0010399 -0.00761295
  40. -0.00424207 0.999991 0.000701576 0.0219133
  41. 0.00104286 -0.000697163 0.999999 0.000314394
  42. 0 0 0 1
  43. shift: 0.023198
  44. -----------------------------------------------------------------
  45. (Processed/Input): (2 / 6)
  46. -----------------------------------------------------------------
  47. Sequence number: 4932
  48. Number of scan points: 6294 points.
  49. Number of filtered scan points: 721 points.
  50. transformed_scan_ptr: 6294 points.
  51. map: 6294 points.
  52. NDT has converged: 1
  53. Fitness score: 24.2187
  54. Number of iteration: 2
  55. (x,y,z,roll,pitch,yaw):
  56. (-0.0141849, 0.012398, -0.00130411, -0.000308826, -0.000888515, -0.00465285)
  57. Transformation Matrix:
  58. 0.999989 0.00465311 -0.000887068 -0.0141849
  59. -0.00465283 0.999989 0.000312956 0.012398
  60. 0.000888515 -0.000308826 1 -0.00130411
  61. 0 0 0 1
  62. shift: 0.0188393
  63. -----------------------------------------------------------------
  64. (Processed/Input): (3 / 6)
  65. -----------------------------------------------------------------
  66. Sequence number: 4933
  67. Number of scan points: 6294 points.
  68. Number of filtered scan points: 720 points.
  69. transformed_scan_ptr: 6294 points.
  70. map: 6294 points.
  71. NDT has converged: 1
  72. Fitness score: 24.2173
  73. Number of iteration: 2
  74. (x,y,z,roll,pitch,yaw):
  75. (-0.0157019, 0.0194076, -0.000536648, -0.000326968, -0.00110621, -0.00371196)
  76. Transformation Matrix:
  77. 0.999992 0.00371232 -0.00110498 -0.0157019
  78. -0.00371195 0.999993 0.00033108 0.0194076
  79. 0.00110621 -0.000326968 0.999999 -0.000536648
  80. 0 0 0 1
  81. shift: 0.0249641
  82. -----------------------------------------------------------------
  83. (Processed/Input): (4 / 6)
  84. -----------------------------------------------------------------
  85. Sequence number: 4934
  86. Number of scan points: 6294 points.
  87. Number of filtered scan points: 720 points.
  88. transformed_scan_ptr: 6294 points.
  89. map: 6294 points.
  90. NDT has converged: 1
  91. Fitness score: 24.1739
  92. Number of iteration: 2
  93. (x,y,z,roll,pitch,yaw):
  94. (-0.00804324, 0.0235166, 0.000151243, -0.000252284, -0.0011184, -0.00308572)
  95. Transformation Matrix:
  96. 0.999995 0.003086 -0.00111762 -0.00804324
  97. -0.00308571 0.999995 0.000255737 0.0235166
  98. 0.0011184 -0.000252284 0.999999 0.000151243
  99. 0 0 0 1
  100. shift: 0.0248541
  101. -----------------------------------------------------------------
  102. (Processed/Input): (5 / 6)
  103. -----------------------------------------------------------------
  104. Sequence number: 4935
  105. Number of scan points: 6294 points.
  106. Number of filtered scan points: 720 points.
  107. transformed_scan_ptr: 6294 points.
  108. map: 6294 points.
  109. NDT has converged: 1
  110. Fitness score: 24.2136
  111. Number of iteration: 2
  112. (x,y,z,roll,pitch,yaw):
  113. (-0.0200421, 0.0201578, -0.000512306, -0.000326471, -0.00110614, -0.00351432)
  114. Transformation Matrix:
  115. 0.999993 0.00351467 -0.00110498 -0.0200421
  116. -0.00351431 0.999994 0.000330364 0.0201578
  117. 0.00110614 -0.000326471 0.999999 -0.000512306
  118. 0 0 0 1
  119. shift: 0.0284258
  120. -----------------------------------------------------------------
  121. (Processed/Input): (6 / 6)
  122. -----------------------------------------------------------------
  123. Sequence number: 4936
  124. Number of scan points: 6278 points.
  125. Number of filtered scan points: 719 points.
  126. transformed_scan_ptr: 6278 points.
  127. map: 6294 points.
  128. NDT has converged: 1
  129. Fitness score: 24.2679
  130. Number of iteration: 2
  131. (x,y,z,roll,pitch,yaw):
  132. (-0.0148786, 0.015239, -0.000179254, -0.000286315, -0.00212471, -0.00333835)
  133. Transformation Matrix:
  134. 0.999992 0.00333895 -0.00212374 -0.0148786
  135. -0.00333834 0.999994 0.000293404 0.015239
  136. 0.00212471 -0.000286315 0.999998 -0.000179254
  137. 0 0 0 1
  138. shift: 0.0212979
  139. -----------------------------------------------------------------
  140. (Processed/Input): (7 / 7)
  141. -----------------------------------------------------------------
  142. Sequence number: 4937
  143. Number of scan points: 6245 points.
  144. Number of filtered scan points: 719 points.
  145. transformed_scan_ptr: 6245 points.
  146. map: 6294 points.
  147. NDT has converged: 1
  148. Fitness score: 23.9591
  149. Number of iteration: 2
  150. (x,y,z,roll,pitch,yaw):
  151. (-0.0211869, 0.0205668, -0.00108052, -0.000390523, -0.00368379, -0.00554739)
  152. Transformation Matrix:
  153. 0.999978 0.0055488 -0.00368156 -0.0211869
  154. -0.00554733 0.999985 0.000410934 0.0205668
  155. 0.00368379 -0.00039052 0.999993 -0.00108052
  156. 0 0 0 1
  157. shift: 0.0295276
  158. -----------------------------------------------------------------
  159. (Processed/Input): (8 / 8)
  160. -----------------------------------------------------------------
  161. Sequence number: 4938
  162. Number of scan points: 6220 points.
  163. Number of filtered scan points: 733 points.
  164. transformed_scan_ptr: 6220 points.
  165. map: 6294 points.
  166. NDT has converged: 1
  167. Fitness score: 24.0271
  168. Number of iteration: 2
  169. (x,y,z,roll,pitch,yaw):
  170. (-0.00227749, 0.0160105, -0.00056129, -0.000546168, -0.0046666, -0.00280137)
  171. Transformation Matrix:
  172. 0.999985 0.00280392 -0.00466503 -0.00227749
  173. -0.00280134 0.999996 0.000559235 0.0160105
  174. 0.00466658 -0.000546162 0.999989 -0.00056129
  175. 0 0 0 1
  176. shift: 0.0161717
  177. -----------------------------------------------------------------
  178. (Processed/Input): (9 / 9)
  179. -----------------------------------------------------------------
  180. Sequence number: 4939
  181. Number of scan points: 6268 points.
  182. Number of filtered scan points: 718 points.
  183. transformed_scan_ptr: 6268 points.
  184. map: 6294 points.
  185. NDT has converged: 1
  186. Fitness score: 23.8963
  187. Number of iteration: 2
  188. (x,y,z,roll,pitch,yaw):
  189. (0.0302971, 0.0110519, -0.00141446, -0.000300501, -0.00428942, -0.00117926)
  190. Transformation Matrix:
  191. 0.99999 0.00118055 -0.00428905 0.0302971
  192. -0.00117925 0.999999 0.000305566 0.0110519
  193. 0.00428941 -0.000300498 0.999991 -0.00141446
  194. 0 0 0 1
  195. shift: 0.0322499
  196. -----------------------------------------------------------------
  197. (Processed/Input): (10 / 10)
  198. -----------------------------------------------------------------
  199. Sequence number: 4940
  200. Number of scan points: 6303 points.
  201. Number of filtered scan points: 745 points.
  202. transformed_scan_ptr: 6303 points.
  203. map: 6294 points.
  204. NDT has converged: 1
  205. Fitness score: 24.0083
  206. Number of iteration: 2
  207. (x,y,z,roll,pitch,yaw):
  208. (0.0806567, 0.0113927, 0.00196571, -0.00238782, -0.00496345, -0.00164691)
  209. Transformation Matrix:
  210. 0.999986 0.00165874 -0.00495948 0.0806567
  211. -0.00164689 0.999996 0.00239598 0.0113927
  212. 0.00496343 -0.00238779 0.999985 0.00196571
  213. 0 0 0 1
  214. shift: 0.0814573
  215. -----------------------------------------------------------------
  216. (Processed/Input): (11 / 11)
  217. -----------------------------------------------------------------
  218. Sequence number: 4941
  219. Number of scan points: 6311 points.
  220. Number of filtered scan points: 746 points.
  221. transformed_scan_ptr: 6311 points.
  222. map: 6294 points.
  223. NDT has converged: 1
  224. Fitness score: 24.608
  225. Number of iteration: 3
  226. (x,y,z,roll,pitch,yaw):
  227. (0.0934048, 0.0243031, 0.00329029, -0.00253165, -0.0047979, -0.0019888)
  228. Transformation Matrix:
  229. 0.999987 0.00200093 -0.00479282 0.0934048
  230. -0.00198878 0.999995 0.00254118 0.0243031
  231. 0.00479788 -0.00253162 0.999985 0.00329029
  232. 0 0 0 1
  233. shift: 0.0965148
  234. -----------------------------------------------------------------
  235. (Processed/Input): (12 / 12)
  236. -----------------------------------------------------------------
  237. Sequence number: 4942
  238. Number of scan points: 6345 points.
  239. Number of filtered scan points: 738 points.
  240. transformed_scan_ptr: 6345 points.
  241. map: 6294 points.
  242. NDT has converged: 1
  243. Fitness score: 23.8523
  244. Number of iteration: 2
  245. (x,y,z,roll,pitch,yaw):
  246. (0.0882098, 0.0211448, 0.00576862, -0.00363352, -0.00423749, -0.00233246)
  247. Transformation Matrix:
  248. 0.999988 0.00234783 -0.00422897 0.0882098
  249. -0.00233244 0.999991 0.00364338 0.0211448
  250. 0.00423748 -0.00363348 0.999984 0.00576862
  251. 0 0 0 1
  252. shift: 0.0907087
  253. -----------------------------------------------------------------
  254. (Processed/Input): (13 / 13)
  255. -----------------------------------------------------------------
  256. Sequence number: 4943
  257. Number of scan points: 6396 points.
  258. Number of filtered scan points: 733 points.
  259. transformed_scan_ptr: 6396 points.
  260. map: 6294 points.
  261. NDT has converged: 1
  262. Fitness score: 23.9805
  263. Number of iteration: 4
  264. (x,y,z,roll,pitch,yaw):
  265. (0.0374861, 0.0299794, 0.00507022, -0.00416566, -0.00448529, -0.00353341)
  266. Transformation Matrix:
  267. 0.999984 0.00355208 -0.00447048 0.0374861
  268. -0.00353337 0.999985 0.00418149 0.0299794
  269. 0.00448528 -0.00416561 0.999981 0.00507022
  270. 0 0 0 1
  271. shift: 0.0479997
  272. -----------------------------------------------------------------
  273. (Processed/Input): (14 / 14)
  274. -----------------------------------------------------------------
  275. Sequence number: 4944
  276. Number of scan points: 6420 points.
  277. Number of filtered scan points: 747 points.
  278. transformed_scan_ptr: 6420 points.
  279. map: 6294 points.
  280. NDT has converged: 1
  281. Fitness score: 23.5403
  282. Number of iteration: 2
  283. (x,y,z,roll,pitch,yaw):
  284. (0.0138763, 0.0166671, 0.00605405, -0.00416391, -0.00508014, -0.00153586)
  285. Transformation Matrix:
  286. 0.999986 0.001557 -0.00507368 0.0138763
  287. -0.00153584 0.99999 0.00417169 0.0166671
  288. 0.00508012 -0.00416384 0.999978 0.00605405
  289. 0 0 0 1
  290. shift: 0.0216874
  291. -----------------------------------------------------------------
  292. (Processed/Input): (15 / 15)
  293. -----------------------------------------------------------------
  294. Sequence number: 4945
  295. Number of scan points: 6436 points.
  296. Number of filtered scan points: 758 points.
  297. transformed_scan_ptr: 6436 points.
  298. map: 6294 points.
  299. NDT has converged: 1
  300. Fitness score: 23.677
  301. Number of iteration: 2
  302. (x,y,z,roll,pitch,yaw):
  303. (0.015063, 0.00615291, 0.00685984, -0.00288157, -0.00364399, -0.00415332)
  304. Transformation Matrix:
  305. 0.999985 0.00416379 -0.00363198 0.015063
  306. -0.00415328 0.999987 0.00289667 0.00615291
  307. 0.00364399 -0.00288155 0.999989 0.00685984
  308. 0 0 0 1
  309. shift: 0.0162712
  310. -----------------------------------------------------------------
  311. (Processed/Input): (16 / 16)
  312. -----------------------------------------------------------------
  313. Sequence number: 4946
  314. Number of scan points: 6602 points.
  315. Number of filtered scan points: 752 points.
  316. transformed_scan_ptr: 6602 points.
  317. map: 6294 points.
  318. NDT has converged: 1
  319. Fitness score: 22.9959
  320. Number of iteration: 2
  321. (x,y,z,roll,pitch,yaw):
  322. (0.0276875, -0.00139905, 0.00806492, -0.00379383, -0.00146751, -0.00197656)
  323. Transformation Matrix:
  324. 0.999997 0.00198213 -0.00145998 0.0276875
  325. -0.00197656 0.999991 0.00379672 -0.00139905
  326. 0.00146751 -0.00379381 0.999992 0.00806492
  327. 0 0 0 1
  328. shift: 0.0277229
  329. -----------------------------------------------------------------
  330. (Processed/Input): (17 / 17)
  331. -----------------------------------------------------------------
  332. Sequence number: 4947
  333. Number of scan points: 6602 points.
  334. Number of filtered scan points: 759 points.
  335. transformed_scan_ptr: 6602 points.
  336. map: 6294 points.
  337. NDT has converged: 1
  338. Fitness score: 23.4046
  339. Number of iteration: 3
  340. (x,y,z,roll,pitch,yaw):
  341. (0.0183337, 0.00405092, 0.00437204, -0.002592, -0.00151861, -0.00503991)
  342. Transformation Matrix:
  343. 0.999986 0.00504381 -0.00150552 0.0183337
  344. -0.00503988 0.999984 0.00259962 0.00405092
  345. 0.00151861 -0.002592 0.999995 0.00437204
  346. 0 0 0 1
  347. shift: 0.0187759
  348. -----------------------------------------------------------------
  349. (Processed/Input): (18 / 18)
  350. -----------------------------------------------------------------
  351. Sequence number: 4948
  352. Number of scan points: 6666 points.
  353. Number of filtered scan points: 775 points.
  354. transformed_scan_ptr: 6666 points.
  355. map: 6294 points.
  356. NDT has converged: 1
  357. Fitness score: 23.678
  358. Number of iteration: 2
  359. (x,y,z,roll,pitch,yaw):
  360. (0.0155032, -0.012682, 0.000432467, -0.0017729, -0.00168608, -0.00205089)
  361. Transformation Matrix:
  362. 0.999996 0.00205389 -0.00168242 0.0155032
  363. -0.00205089 0.999996 0.00177638 -0.012682
  364. 0.00168608 -0.0017729 0.999997 0.000432467
  365. 0 0 0 1
  366. shift: 0.0200295
  367. -----------------------------------------------------------------
  368. (Processed/Input): (19 / 19)
  369. -----------------------------------------------------------------
  370. Sequence number: 4949
  371. Number of scan points: 6694 points.
  372. Number of filtered scan points: 765 points.
  373. transformed_scan_ptr: 6694 points.
  374. map: 6294 points.
  375. NDT has converged: 1
  376. Fitness score: 22.9958
  377. Number of iteration: 2
  378. (x,y,z,roll,pitch,yaw):
  379. (0.0313035, -0.00436905, 0.0023816, -0.00351089, -0.00271923, -0.000512669)
  380. Transformation Matrix:
  381. 0.999996 0.000522232 -0.0027174 0.0313035
  382. -0.000512667 0.999994 0.00351228 -0.00436905
  383. 0.00271922 -0.00351087 0.99999 0.0023816
  384. 0 0 0 1
  385. shift: 0.031607
  386. -----------------------------------------------------------------
  387. (Processed/Input): (20 / 20)
  388. -----------------------------------------------------------------
  389. Sequence number: 4950
  390. Number of scan points: 6747 points.
  391. Number of filtered scan points: 788 points.
  392. transformed_scan_ptr: 6747 points.
  393. map: 6294 points.
  394. NDT has converged: 1
  395. Fitness score: 22.6316
  396. Number of iteration: 2
  397. (x,y,z,roll,pitch,yaw):
  398. (0.0209151, 0.0128721, 0.00273712, -0.00476761, -0.00115091, 0.00107209)
  399. Transformation Matrix:
  400. 0.999999 -0.0010666 -0.001156 0.0209151
  401. 0.00107209 0.999988 0.00476636 0.0128721
  402. 0.00115091 -0.00476759 0.999988 0.00273712
  403. 0 0 0 1
  404. shift: 0.0245588
  405. -----------------------------------------------------------------
  406. (Processed/Input): (21 / 21)
  407. -----------------------------------------------------------------
  408. Sequence number: 4951
  409. Number of scan points: 6775 points.
  410. Number of filtered scan points: 780 points.
  411. transformed_scan_ptr: 6775 points.
  412. map: 6294 points.
  413. NDT has converged: 1
  414. Fitness score: 23.2085
  415. Number of iteration: 4
  416. (x,y,z,roll,pitch,yaw):
  417. (-0.0507711, 0.00203508, 0.000321334, -0.00527809, -0.00114955, -0.0127606)
  418. Transformation Matrix:
  419. 0.999918 0.0127661 -0.0010821 -0.0507711
  420. -0.0127602 0.999905 0.0052923 0.00203508
  421. 0.00114955 -0.00527806 0.999985 0.000321334
  422. 0 0 0 1
  423. shift: 0.0508118
  424. -----------------------------------------------------------------
  425. (Processed/Input): (22 / 22)
  426. -----------------------------------------------------------------
  427. Sequence number: 4952
  428. Number of scan points: 6843 points.
  429. Number of filtered scan points: 800 points.
  430. transformed_scan_ptr: 6843 points.
  431. map: 6294 points.
  432. NDT has converged: 1
  433. Fitness score: 24.3348
  434. Number of iteration: 4
  435. (x,y,z,roll,pitch,yaw):
  436. (-0.0261301, -0.040477, 5.92529e-06, -0.00594404, 0.00074277, -0.0146938)
  437. Transformation Matrix:
  438. 0.999892 0.0146886 0.000830031 -0.0261301
  439. -0.0146933 0.999874 0.00593244 -0.040477
  440. -0.00074277 -0.005944 0.999982 5.92529e-06
  441. 0 0 0 1
  442. shift: 0.0481785
  443. -----------------------------------------------------------------
  444. (Processed/Input): (23 / 23)
  445. -----------------------------------------------------------------
  446. Sequence number: 4953
  447. Number of scan points: 6902 points.
  448. Number of filtered scan points: 799 points.
  449. transformed_scan_ptr: 6902 points.
  450. map: 6294 points.
  451. NDT has converged: 1
  452. Fitness score: 23.9076
  453. Number of iteration: 3
  454. (x,y,z,roll,pitch,yaw):
  455. (0.0086859, -0.0496417, -0.00339407, -0.00489111, 0.000360717, -0.012859)
  456. Transformation Matrix:
  457. 0.999917 0.0128568 0.000423589 0.0086859
  458. -0.0128587 0.999905 0.00488604 -0.0496417
  459. -0.000360717 -0.00489109 0.999988 -0.00339407
  460. 0 0 0 1
  461. shift: 0.0503959
  462. -----------------------------------------------------------------
  463. (Processed/Input): (24 / 24)
  464. -----------------------------------------------------------------
  465. Sequence number: 4954
  466. Number of scan points: 7013 points.
  467. Number of filtered scan points: 822 points.
  468. transformed_scan_ptr: 7013 points.
  469. map: 6294 points.
  470. NDT has converged: 1
  471. Fitness score: 23.9787
  472. Number of iteration: 3
  473. (x,y,z,roll,pitch,yaw):
  474. (0.0390956, -0.016512, -0.0034589, -0.0066735, -0.000852138, -0.00541789)
  475. Transformation Matrix:
  476. 0.999985 0.00542343 -0.000815961 0.0390956
  477. -0.00541786 0.999963 0.00667796 -0.016512
  478. 0.000852138 -0.00667344 0.999977 -0.0034589
  479. 0 0 0 1
  480. shift: 0.0424395
  481. -----------------------------------------------------------------
  482. (Processed/Input): (25 / 25)
  483. -----------------------------------------------------------------
  484. Sequence number: 4955
  485. Number of scan points: 6969 points.
  486. Number of filtered scan points: 830 points.
  487. transformed_scan_ptr: 6969 points.
  488. map: 6294 points.
  489. NDT has converged: 1
  490. Fitness score: 23.7828
  491. Number of iteration: 3
  492. (x,y,z,roll,pitch,yaw):
  493. (0.012103, -0.019611, -0.00288442, -0.00805956, -0.00235118, -0.00525107)
  494. Transformation Matrix:
  495. 0.999983 0.00526981 -0.00230877 0.012103
  496. -0.00525103 0.999954 0.0080717 -0.019611
  497. 0.00235118 -0.00805945 0.999965 -0.00288442
  498. 0 0 0 1
  499. shift: 0.0230451
  500. -----------------------------------------------------------------
  501. (Processed/Input): (26 / 26)
  502. -----------------------------------------------------------------
  503. Sequence number: 4956
  504. Number of scan points: 7057 points.
  505. Number of filtered scan points: 838 points.
  506. transformed_scan_ptr: 7057 points.
  507. map: 6294 points.
  508. NDT has converged: 1
  509. Fitness score: 23.2812
  510. Number of iteration: 2
  511. (x,y,z,roll,pitch,yaw):
  512. (-0.00937515, -0.00575047, -0.00709405, -0.0095277, -0.00286373, -0.00515607)
  513. Transformation Matrix:
  514. 0.999983 0.00518308 -0.00281445 -0.00937515
  515. -0.00515602 0.999941 0.00954219 -0.00575047
  516. 0.00286372 -0.00952752 0.999951 -0.00709405
  517. 0 0 0 1
  518. shift: 0.0109982
  519. -----------------------------------------------------------------
  520. (Processed/Input): (27 / 27)
  521. -----------------------------------------------------------------
  522. Sequence number: 4957
  523. Number of scan points: 7137 points.
  524. Number of filtered scan points: 846 points.
  525. transformed_scan_ptr: 7137 points.
  526. map: 6294 points.
  527. NDT has converged: 1
  528. Fitness score: 22.803
  529. Number of iteration: 2
  530. (x,y,z,roll,pitch,yaw):
  531. (-0.0336586, 0.00732984, -0.00154852, -0.0145293, -0.00607615, -0.0134495)
  532. Transformation Matrix:
  533. 0.999891 0.013536 -0.00587951 -0.0336586
  534. -0.0134489 0.999803 0.0146092 0.00732984
  535. 0.00607612 -0.0145285 0.999876 -0.00154852
  536. 0 0 0 1
  537. shift: 0.0344474
  538. -----------------------------------------------------------------
  539. (Processed/Input): (28 / 28)
  540. -----------------------------------------------------------------
  541. Sequence number: 4958
  542. Number of scan points: 7117 points.
  543. Number of filtered scan points: 853 points.
  544. transformed_scan_ptr: 7117 points.
  545. map: 6294 points.
  546. NDT has converged: 1
  547. Fitness score: 23.0311
  548. Number of iteration: 3
  549. (x,y,z,roll,pitch,yaw):
  550. (-0.036443, -0.00320715, -0.00286627, -0.0188436, -0.00683064, -0.00361707)
  551. Transformation Matrix:
  552. 0.99997 0.00374514 -0.00676117 -0.036443
  553. -0.00361698 0.999815 0.018867 -0.00320715
  554. 0.00683059 -0.018842 0.999799 -0.00286627
  555. 0 0 0 1
  556. shift: 0.0365839
  557. -----------------------------------------------------------------
  558. (Processed/Input): (29 / 29)
  559. -----------------------------------------------------------------
  560. Sequence number: 4959
  561. Number of scan points: 7205 points.
  562. Number of filtered scan points: 878 points.
  563. transformed_scan_ptr: 7205 points.
  564. map: 6294 points.
  565. NDT has converged: 1
  566. Fitness score: 24.2081
  567. Number of iteration: 3
  568. (x,y,z,roll,pitch,yaw):
  569. (0.0130067, 0.0314802, -0.00391934, -0.0202981, -0.00680795, -0.00356997)
  570. Transformation Matrix:
  571. 0.99997 0.00370741 -0.00673399 0.0130067
  572. -0.00356988 0.999787 0.0203208 0.0314802
  573. 0.0068079 -0.0202962 0.999771 -0.00391934
  574. 0 0 0 1
  575. shift: 0.0340614
  576. -----------------------------------------------------------------
  577. (Processed/Input): (30 / 30)
  578. -----------------------------------------------------------------
  579. Sequence number: 4960
  580. Number of scan points: 7235 points.
  581. Number of filtered scan points: 911 points.
  582. transformed_scan_ptr: 7235 points.
  583. map: 6294 points.
  584. NDT has converged: 1
  585. Fitness score: 24.3429
  586. Number of iteration: 3
  587. (x,y,z,roll,pitch,yaw):
  588. (-0.00680624, 0.0326962, -0.000544223, -0.0220987, -0.00723061, -0.00473637)
  589. Transformation Matrix:
  590. 0.999963 0.00489499 -0.00712403 -0.00680624
  591. -0.00473623 0.999744 0.0221309 0.0326962
  592. 0.00723055 -0.0220963 0.99973 -0.000544223
  593. 0 0 0 1
  594. shift: 0.0333971
  595. -----------------------------------------------------------------
  596. (Processed/Input): (31 / 31)
  597. -----------------------------------------------------------------
  598. Sequence number: 4961
  599. Number of scan points: 7248 points.
  600. Number of filtered scan points: 930 points.
  601. transformed_scan_ptr: 7248 points.
  602. map: 6294 points.
  603. NDT has converged: 1
  604. Fitness score: 24.0743
  605. Number of iteration: 3
  606. (x,y,z,roll,pitch,yaw):
  607. (-0.00217275, 0.0201667, 0.000349513, -0.0242102, -0.00711427, -0.0127205)
  608. Transformation Matrix:
  609. 0.999894 0.0128886 -0.0068036 -0.00217275
  610. -0.0127198 0.999624 0.0242963 0.0201667
  611. 0.00711421 -0.0242072 0.999682 0.000349513
  612. 0 0 0 1
  613. shift: 0.0202834
  614. -----------------------------------------------------------------
  615. (Processed/Input): (32 / 32)
  616. -----------------------------------------------------------------
  617. Sequence number: 4962
  618. Number of scan points: 7287 points.
  619. Number of filtered scan points: 954 points.
  620. transformed_scan_ptr: 7287 points.
  621. map: 6294 points.
  622. NDT has converged: 1
  623. Fitness score: 24.9756
  624. Number of iteration: 2
  625. (x,y,z,roll,pitch,yaw):
  626. (0.00789677, 0.0312003, 0.00216979, -0.0260461, -0.00779711, -0.0116122)
  627. Transformation Matrix:
  628. 0.999902 0.0118111 -0.00749141 0.00789677
  629. -0.0116116 0.999591 0.0261319 0.0312003
  630. 0.00779703 -0.0260423 0.99963 0.00216979
  631. 0 0 0 1
  632. shift: 0.0321841
  633. -----------------------------------------------------------------
  634. (Processed/Input): (33 / 33)
  635. -----------------------------------------------------------------
  636. Sequence number: 4963
  637. Number of scan points: 7356 points.
  638. Number of filtered scan points: 967 points.
  639. transformed_scan_ptr: 7356 points.
  640. map: 6294 points.
  641. NDT has converged: 1
  642. Fitness score: 24.3343
  643. Number of iteration: 2
  644. (x,y,z,roll,pitch,yaw):
  645. (0.000417699, 0.0371699, 0.00362906, -0.0235014, -0.00648531, -0.00803877)
  646. Transformation Matrix:
  647. 0.999947 0.00818886 -0.00629436 0.000417699
  648. -0.00803852 0.99969 0.0235506 0.0371699
  649. 0.00648526 -0.0234988 0.999703 0.00362906
  650. 0 0 0 1
  651. shift: 0.0371722
  652. -----------------------------------------------------------------
  653. (Processed/Input): (34 / 34)
  654. -----------------------------------------------------------------
  655. Sequence number: 4964
  656. Number of scan points: 7394 points.
  657. Number of filtered scan points: 950 points.
  658. transformed_scan_ptr: 7394 points.
  659. map: 6294 points.
  660. NDT has converged: 1
  661. Fitness score: 23.9195
  662. Number of iteration: 2
  663. (x,y,z,roll,pitch,yaw):
  664. (-0.0137697, 0.00832355, 0.00838804, -0.0167046, -0.0027272, -0.00808028)
  665. Transformation Matrix:
  666. 0.999964 0.00812462 -0.00259176 -0.0137697
  667. -0.00808017 0.999827 0.0167253 0.00832355
  668. 0.0027272 -0.0167038 0.999857 0.00838804
  669. 0 0 0 1
  670. shift: 0.0160899
  671. -----------------------------------------------------------------
  672. (Processed/Input): (35 / 35)
  673. -----------------------------------------------------------------
  674. Sequence number: 4965
  675. Number of scan points: 7416 points.
  676. Number of filtered scan points: 960 points.
  677. transformed_scan_ptr: 7416 points.
  678. map: 6294 points.
  679. NDT has converged: 1
  680. Fitness score: 24.461
  681. Number of iteration: 2
  682. (x,y,z,roll,pitch,yaw):
  683. (0.0125231, -0.0193704, 0.0113391, -0.011091, 0.00161315, -0.00470325)
  684. Transformation Matrix:
  685. 0.999988 0.00468505 0.00166521 0.0125231
  686. -0.00470323 0.999928 0.011083 -0.0193704
  687. -0.00161315 -0.0110907 0.999937 0.0113391
  688. 0 0 0 1
  689. shift: 0.023066
  690. -----------------------------------------------------------------
  691. (Processed/Input): (36 / 36)
  692. -----------------------------------------------------------------
  693. Sequence number: 4966
  694. Number of scan points: 7398 points.
  695. Number of filtered scan points: 980 points.
  696. transformed_scan_ptr: 7398 points.
  697. map: 6294 points.
  698. NDT has converged: 1
  699. Fitness score: 24.5543
  700. Number of iteration: 3
  701. (x,y,z,roll,pitch,yaw):
  702. (-0.00122374, -0.053501, 0.00902797, -0.00366143, 0.00365695, -0.00459523)
  703. Transformation Matrix:
  704. 0.999983 0.0045818 0.00367369 -0.00122374
  705. -0.00459518 0.999983 0.00364459 -0.053501
  706. -0.00365694 -0.0036614 0.999987 0.00902797
  707. 0 0 0 1
  708. shift: 0.053515
  709. -----------------------------------------------------------------
  710. (Processed/Input): (37 / 37)
  711. -----------------------------------------------------------------
  712. Sequence number: 4967
  713. Number of scan points: 7388 points.
  714. Number of filtered scan points: 976 points.
  715. transformed_scan_ptr: 7388 points.
  716. map: 6294 points.
  717. NDT has converged: 1
  718. Fitness score: 24.1215
  719. Number of iteration: 2
  720. (x,y,z,roll,pitch,yaw):
  721. (0.00793347, -0.0610355, 0.00102758, 0.0051402, 0.0035855, -0.00605548)
  722. Transformation Matrix:
  723. 0.999975 0.00607379 0.00355425 0.00793347
  724. -0.0060554 0.999968 -0.00516179 -0.0610355
  725. -0.00358549 0.00514014 0.99998 0.00102758
  726. 0 0 0 1
  727. shift: 0.0615489
  728. -----------------------------------------------------------------
  729. (Processed/Input): (38 / 38)
  730. -----------------------------------------------------------------
  731. Sequence number: 4968
  732. Number of scan points: 7353 points.
  733. Number of filtered scan points: 980 points.
  734. transformed_scan_ptr: 7353 points.
  735. map: 6294 points.
  736. NDT has converged: 1
  737. Fitness score: 24.0353
  738. Number of iteration: 2
  739. (x,y,z,roll,pitch,yaw):
  740. (0.0140941, -0.0624637, -0.00818659, 0.00948135, 0.0043787, -0.00528619)
  741. Transformation Matrix:
  742. 0.999976 0.00532744 0.00432832 0.0140941
  743. -0.00528612 0.999941 -0.00950422 -0.0624637
  744. -0.00437869 0.00948112 0.999945 -0.00818659
  745. 0 0 0 1
  746. shift: 0.0640341
  747. -----------------------------------------------------------------
  748. (Processed/Input): (39 / 39)
  749. -----------------------------------------------------------------
  750. Sequence number: 4969
  751. Number of scan points: 7385 points.
  752. Number of filtered scan points: 1005 points.
  753. transformed_scan_ptr: 7385 points.
  754. map: 6294 points.
  755. NDT has converged: 1
  756. Fitness score: 24.0632
  757. Number of iteration: 2
  758. (x,y,z,roll,pitch,yaw):
  759. (0.041579, -0.073511, -0.0255607, 0.00983519, 0.00638885, -0.00371003)
  760. Transformation Matrix:
  761. 0.999973 0.00377266 0.00635197 0.041579
  762. -0.00370994 0.999945 -0.00985866 -0.073511
  763. -0.00638881 0.00983483 0.999931 -0.0255607
  764. 0 0 0 1
  765. shift: 0.0844552
  766. -----------------------------------------------------------------
  767. (Processed/Input): (40 / 40)
  768. -----------------------------------------------------------------
  769. Sequence number: 4970
  770. Number of scan points: 7282 points.
  771. Number of filtered scan points: 1022 points.
  772. transformed_scan_ptr: 7282 points.
  773. map: 6294 points.
  774. NDT has converged: 1
  775. Fitness score: 24.6699
  776. Number of iteration: 2
  777. (x,y,z,roll,pitch,yaw):
  778. (0.0544338, -0.0713245, -0.0376744, 0.00530921, 0.00738133, -0.00384677)
  779. Transformation Matrix:
  780. 0.999965 0.0038859 0.00736068 0.0544338
  781. -0.00384665 0.999978 -0.00533755 -0.0713245
  782. -0.00738126 0.00530904 0.999959 -0.0376744
  783. 0 0 0 1
  784. shift: 0.089723
  785. -----------------------------------------------------------------
  786. (Processed/Input): (41 / 41)
  787. -----------------------------------------------------------------
  788. Sequence number: 4971
  789. Number of scan points: 7360 points.
  790. Number of filtered scan points: 1058 points.
  791. transformed_scan_ptr: 7360 points.
  792. map: 6294 points.
  793. NDT has converged: 1
  794. Fitness score: 25.6286
  795. Number of iteration: 3
  796. (x,y,z,roll,pitch,yaw):
  797. (0.052128, 0.00234396, -0.0644927, -0.00818862, 0.0122714, -0.000857566)
  798. Transformation Matrix:
  799. 0.999924 0.000757061 0.0122777 0.052128
  800. -0.000857502 0.999966 0.008178 0.00234396
  801. -0.012271 -0.00818791 0.999891 -0.0644927
  802. 0 0 0 1
  803. shift: 0.0521807
  804. -----------------------------------------------------------------
  805. (Processed/Input): (42 / 42)
  806. -----------------------------------------------------------------
  807. Sequence number: 4972
  808. Number of scan points: 7193 points.
  809. Number of filtered scan points: 1045 points.
  810. transformed_scan_ptr: 7193 points.
  811. map: 6294 points.
  812. NDT has converged: 1
  813. Fitness score: 25.7074
  814. Number of iteration: 3
  815. (x,y,z,roll,pitch,yaw):
  816. (0.0203013, -0.0254789, -0.0810399, -0.0158213, 0.0117789, -0.00914004)
  817. Transformation Matrix:
  818. 0.999889 0.00895242 0.0119213 0.0203013
  819. -0.00913928 0.999835 0.0157123 -0.0254789
  820. -0.0117786 -0.0158195 0.999806 -0.0810399
  821. 0 0 0 1
  822. shift: 0.0325779
  823. -----------------------------------------------------------------
  824. (Processed/Input): (43 / 43)
  825. -----------------------------------------------------------------
  826. Sequence number: 4973
  827. Number of scan points: 7224 points.
  828. Number of filtered scan points: 1051 points.
  829. transformed_scan_ptr: 7224 points.
  830. map: 6294 points.
  831. NDT has converged: 1
  832. Fitness score: 24.6305
  833. Number of iteration: 2
  834. (x,y,z,roll,pitch,yaw):
  835. (-0.00823359, -0.0683582, -0.100731, -0.0196266, 0.0112093, -0.00841853)
  836. Transformation Matrix:
  837. 0.999902 0.00819684 0.0113718 -0.00823359
  838. -0.0084179 0.999774 0.0195303 -0.0683582
  839. -0.0112091 -0.0196241 0.999745 -0.100731
  840. 0 0 0 1
  841. shift: 0.0688523
  842. -----------------------------------------------------------------
  843. (Processed/Input): (44 / 44)
  844. -----------------------------------------------------------------
  845. Sequence number: 4974
  846. Number of scan points: 7239 points.
  847. Number of filtered scan points: 1052 points.
  848. transformed_scan_ptr: 7239 points.
  849. map: 6294 points.
  850. NDT has converged: 1
  851. Fitness score: 25.0688
  852. Number of iteration: 4
  853. (x,y,z,roll,pitch,yaw):
  854. (0.023126, 0.0156373, -0.120908, -0.0153606, 0.00984456, -0.00330062)
  855. Transformation Matrix:
  856. 0.999946 0.00314903 0.00989388 0.023126
  857. -0.00330045 0.999877 0.0153274 0.0156373
  858. -0.0098444 -0.0153593 0.999834 -0.120908
  859. 0 0 0 1
  860. shift: 0.0279166
  861. -----------------------------------------------------------------
  862. (Processed/Input): (45 / 45)
  863. -----------------------------------------------------------------
  864. Sequence number: 4975
  865. Number of scan points: 7266 points.
  866. Number of filtered scan points: 1053 points.
  867. transformed_scan_ptr: 7266 points.
  868. map: 6294 points.
  869. NDT has converged: 1
  870. Fitness score: 26.0534
  871. Number of iteration: 4
  872. (x,y,z,roll,pitch,yaw):
  873. (0.0253935, -0.0385426, -0.132632, -0.0101092, 0.00613759, -0.00167557)
  874. Transformation Matrix:
  875. 0.99998 0.00161343 0.00615417 0.0253935
  876. -0.00167553 0.999948 0.0100987 -0.0385426
  877. -0.00613756 -0.0101088 0.99993 -0.132632
  878. 0 0 0 1
  879. shift: 0.0461559
  880. -----------------------------------------------------------------
  881. (Processed/Input): (46 / 46)
  882. -----------------------------------------------------------------
  883. Sequence number: 4976
  884. Number of scan points: 7135 points.
  885. Number of filtered scan points: 1062 points.
  886. transformed_scan_ptr: 7135 points.
  887. map: 6294 points.
  888. NDT has converged: 1
  889. Fitness score: 25.3247
  890. Number of iteration: 3
  891. (x,y,z,roll,pitch,yaw):
  892. (0.0218474, -0.0244984, -0.137857, 0.0009402, 0.00164904, -0.0020096)
  893. Transformation Matrix:
  894. 0.999997 0.00201114 0.00164715 0.0218474
  895. -0.00200959 0.999998 -0.000943505 -0.0244984
  896. -0.00164904 0.000940198 0.999998 -0.137857
  897. 0 0 0 1
  898. shift: 0.032825
  899. -----------------------------------------------------------------
  900. (Processed/Input): (47 / 47)
  901. (Processed/Input): (48 / 48)
  902. -----------------------------------------------------------------
  903. Sequence number: 4977
  904. Number of scan points: 7105 points.
  905. Number of filtered scan points: 1073 points.
  906. transformed_scan_ptr: 7105 points.
  907. map: 6294 points.
  908. NDT has converged: 1
  909. Fitness score: 26.5665
  910. Number of iteration: 3
  911. (x,y,z,roll,pitch,yaw):
  912. (-0.00572269, -0.0892745, -0.142488, 0.016169, -0.00219547, -0.00784294)
  913. Transformation Matrix:
  914. 0.999967 0.00780634 -0.00232191 -0.00572269
  915. -0.00784284 0.999839 -0.0161506 -0.0892745
  916. 0.00219547 0.0161682 0.999867 -0.142488
  917. 0 0 0 1
  918. shift: 0.0894577
  919. -----------------------------------------------------------------
  920. -----------------------------------------------------------------
  921. Sequence number: 4978
  922. Number of scan points: 7098 points.
  923. Number of filtered scan points: 1058 points.
  924. transformed_scan_ptr: 7098 points.
  925. map: 6294 points.
  926. NDT has converged: 1
  927. Fitness score: 25.6573
  928. Number of iteration: 5
  929. (x,y,z,roll,pitch,yaw):
  930. (0.0460274, -0.096874, -0.140139, 0.0269287, -0.00359205, 0.000605998)
  931. Transformation Matrix:
  932. 0.999993 -0.000702504 -0.00357442 0.0460274
  933. 0.000605994 0.999637 -0.0269276 -0.096874
  934. 0.00359205 0.0269252 0.999631 -0.140139
  935. 0 0 0 1
  936. shift: 0.107252
  937. -----------------------------------------------------------------
  938. (Processed/Input): (49 / 49)
  939. -----------------------------------------------------------------
  940. Sequence number: 4979
  941. Number of scan points: 6947 points.
  942. Number of filtered scan points: 1055 points.
  943. transformed_scan_ptr: 6947 points.
  944. map: 6294 points.
  945. NDT has converged: 1
  946. Fitness score: 26.1356
  947. Number of iteration: 3
  948. (x,y,z,roll,pitch,yaw):
  949. (0.0569511, -0.0801379, -0.138094, 0.0318658, -0.00575092, -0.00298255)
  950. Transformation Matrix:
  951. 0.999979 0.0027978 -0.00584297 0.0569511
  952. -0.0029825 0.999488 -0.0318432 -0.0801379
  953. 0.00575089 0.0318599 0.999476 -0.138094
  954. 0 0 0 1
  955. shift: 0.0983133
  956. -----------------------------------------------------------------
  957. (Processed/Input): (50 / 50)
  958. -----------------------------------------------------------------
  959. Sequence number: 4980
  960. Number of scan points: 6911 points.
  961. Number of filtered scan points: 1091 points.
  962. transformed_scan_ptr: 6911 points.
  963. map: 6294 points.
  964. NDT has converged: 1
  965. Fitness score: 26.787
  966. Number of iteration: 3
  967. (x,y,z,roll,pitch,yaw):
  968. (0.00558316, -0.109254, -0.133365, 0.033642, -0.0077558, -0.00487464)
  969. Transformation Matrix:
  970. 0.999958 0.00461101 -0.0079152 0.00558316
  971. -0.00487448 0.999424 -0.0335975 -0.109254
  972. 0.00775572 0.0336347 0.999404 -0.133365
  973. 0 0 0 1
  974. shift: 0.109397
  975. -----------------------------------------------------------------
  976. (Processed/Input): (51 / 51)
  977. -----------------------------------------------------------------
  978. Sequence number: 4981
  979. Number of scan points: 6973 points.
  980. Number of filtered scan points: 1117 points.
  981. transformed_scan_ptr: 6973 points.
  982. map: 6294 points.
  983. NDT has converged: 1
  984. Fitness score: 27.0445
  985. Number of iteration: 3
  986. (x,y,z,roll,pitch,yaw):
  987. (-0.0169734, -0.0958142, -0.124684, 0.0322864, -0.011018, -0.00956779)
  988. Transformation Matrix:
  989. 0.999894 0.00920701 -0.0113203 -0.0169734
  990. -0.00956707 0.999436 -0.0321739 -0.0958142
  991. 0.0110177 0.0322788 0.999418 -0.124684
  992. 0 0 0 1
  993. shift: 0.097306
  994. -----------------------------------------------------------------
  995. (Processed/Input): (52 / 52)
  996. -----------------------------------------------------------------
  997. Sequence number: 4982
  998. Number of scan points: 6870 points.
  999. Number of filtered scan points: 1080 points.
  1000. transformed_scan_ptr: 6870 points.
  1001. map: 6294 points.
  1002. NDT has converged: 1
  1003. Fitness score: 27.2019
  1004. Number of iteration: 2
  1005. (x,y,z,roll,pitch,yaw):
  1006. (-0.0266886, -0.0781252, -0.111147, 0.032145, -0.00993899, -0.0071321)
  1007. Transformation Matrix:
  1008. 0.999925 0.00680894 -0.0101627 -0.0266886
  1009. -0.00713169 0.99946 -0.0320678 -0.0781252
  1010. 0.00993882 0.0321378 0.999434 -0.111147
  1011. 0 0 0 1
  1012. shift: 0.082558
  1013. -----------------------------------------------------------------
  1014. (Processed/Input): (53 / 53)
  1015. -----------------------------------------------------------------
  1016. Sequence number: 4983
  1017. Number of scan points: 6767 points.
  1018. Number of filtered scan points: 1094 points.
  1019. transformed_scan_ptr: 6767 points.
  1020. map: 6294 points.
  1021. NDT has converged: 1
  1022. Fitness score: 26.643
  1023. Number of iteration: 3
  1024. (x,y,z,roll,pitch,yaw):
  1025. (-0.000820437, -0.125381, -0.103871, 0.0331599, -0.00992735, -0.00280335)
  1026. Transformation Matrix:
  1027. 0.999947 0.00247267 -0.0100146 -0.000820437
  1028. -0.00280321 0.999447 -0.0331259 -0.125381
  1029. 0.00992718 0.0331522 0.999401 -0.103871
  1030. 0 0 0 1
  1031. shift: 0.125383
  1032. -----------------------------------------------------------------
  1033. (Processed/Input): (54 / 54)
  1034. -----------------------------------------------------------------
  1035. Sequence number: 4984
  1036. Number of scan points: 7016 points.
  1037. Number of filtered scan points: 1132 points.
  1038. transformed_scan_ptr: 7016 points.
  1039. map: 6294 points.
  1040. NDT has converged: 1
  1041. Fitness score: 26.2946
  1042. Number of iteration: 4
  1043. (x,y,z,roll,pitch,yaw):
  1044. (-0.0115511, -0.136168, -0.094847, 0.0348202, -0.00717045, -0.00729742)
  1045. Transformation Matrix:
  1046. 0.999948 0.00704332 -0.00741989 -0.0115511
  1047. -0.00729716 0.999369 -0.0347599 -0.136168
  1048. 0.00717039 0.0348122 0.999368 -0.094847
  1049. 0 0 0 1
  1050. shift: 0.136657
  1051. -----------------------------------------------------------------
  1052. (Processed/Input): (55 / 55)
  1053. -----------------------------------------------------------------
  1054. Sequence number: 4985
  1055. Number of scan points: 7036 points.
  1056. Number of filtered scan points: 1156 points.
  1057. transformed_scan_ptr: 7036 points.
  1058. map: 6294 points.
  1059. NDT has converged: 1
  1060. Fitness score: 26.112
  1061. Number of iteration: 4
  1062. (x,y,z,roll,pitch,yaw):
  1063. (-0.039607, -0.0784213, -0.0833986, 0.0348008, -0.00588289, -0.000384491)
  1064. Transformation Matrix:
  1065. 0.999983 0.000179575 -0.00589268 -0.039607
  1066. -0.000384484 0.999395 -0.0347915 -0.0784213
  1067. 0.00588286 0.0347932 0.999377 -0.0833986
  1068. 0 0 0 1
  1069. shift: 0.0878556
  1070. -----------------------------------------------------------------
  1071. (Processed/Input): (56 / 56)
  1072. -----------------------------------------------------------------
  1073. Sequence number: 4986
  1074. Number of scan points: 7001 points.
  1075. Number of filtered scan points: 1160 points.
  1076. transformed_scan_ptr: 7001 points.
  1077. map: 6294 points.
  1078. NDT has converged: 1
  1079. Fitness score: 25.7083
  1080. Number of iteration: 3
  1081. (x,y,z,roll,pitch,yaw):
  1082. (-0.0370514, -0.0821259, -0.0741678, 0.0361037, -0.00476915, -0.000735075)
  1083. Transformation Matrix:
  1084. 0.999988 0.000562455 -0.00479256 -0.0370514
  1085. -0.000735066 0.999348 -0.0360924 -0.0821259
  1086. 0.00476914 0.0360955 0.999337 -0.0741678
  1087. 0 0 0 1
  1088. shift: 0.090097
  1089. -----------------------------------------------------------------
  1090. (Processed/Input): (57 / 57)
  1091. -----------------------------------------------------------------
  1092. Sequence number: 4987
  1093. Number of scan points: 7120 points.
  1094. Number of filtered scan points: 1188 points.
  1095. transformed_scan_ptr: 7120 points.
  1096. map: 6294 points.
  1097. NDT has converged: 1
  1098. Fitness score: 25.8046
  1099. Number of iteration: 3
  1100. (x,y,z,roll,pitch,yaw):
  1101. (0.0327677, -0.087717, -0.0632614, 0.039393, -0.00338143, -0.00239841)
  1102. Transformation Matrix:
  1103. 0.999991 0.00226338 -0.00347325 0.0327677
  1104. -0.00239839 0.999222 -0.0393746 -0.087717
  1105. 0.00338143 0.0393826 0.999219 -0.0632614
  1106. 0 0 0 1
  1107. shift: 0.0936375
  1108. -----------------------------------------------------------------
  1109. (Processed/Input): (58 / 58)
  1110. -----------------------------------------------------------------
  1111. Sequence number: 4988
  1112. Number of scan points: 7019 points.
  1113. Number of filtered scan points: 1195 points.
  1114. transformed_scan_ptr: 7019 points.
  1115. map: 6294 points.
  1116. NDT has converged: 1
  1117. Fitness score: 25.7081
  1118. Number of iteration: 3
  1119. (x,y,z,roll,pitch,yaw):
  1120. (0.0431029, -0.146831, -0.0536982, 0.0461987, -0.00192691, -0.00380408)
  1121. Transformation Matrix:
  1122. 0.999991 0.00371102 -0.00210052 0.0431029
  1123. -0.00380406 0.998926 -0.0461746 -0.146831
  1124. 0.00192691 0.0461822 0.998931 -0.0536982
  1125. 0 0 0 1
  1126. shift: 0.153027
  1127. -----------------------------------------------------------------
  1128. (Processed/Input): (59 / 59)
  1129. -----------------------------------------------------------------
  1130. Sequence number: 4989
  1131. Number of scan points: 7145 points.
  1132. Number of filtered scan points: 1208 points.
  1133. transformed_scan_ptr: 7145 points.
  1134. map: 6294 points.
  1135. NDT has converged: 1
  1136. Fitness score: 25.4996
  1137. Number of iteration: 3
  1138. (x,y,z,roll,pitch,yaw):
  1139. (0.0185299, -0.197299, -0.0530774, 0.055996, 0.00339206, -0.00798309)
  1140. Transformation Matrix:
  1141. 0.999962 0.00816032 0.00293985 0.0185299
  1142. -0.00798296 0.998399 -0.055992 -0.197299
  1143. -0.00339205 0.0559664 0.998427 -0.0530774
  1144. 0 0 0 1
  1145. shift: 0.198168
  1146. -----------------------------------------------------------------
  1147. (Processed/Input): (60 / 60)
  1148. -----------------------------------------------------------------
  1149. Sequence number: 4990
  1150. Number of scan points: 7106 points.
  1151. Number of filtered scan points: 1206 points.
  1152. transformed_scan_ptr: 7106 points.
  1153. map: 6294 points.
  1154. NDT has converged: 1
  1155. Fitness score: 25.3149
  1156. Number of iteration: 6
  1157. (x,y,z,roll,pitch,yaw):
  1158. (0.041356, -0.161631, -0.0470721, 0.065528, 0.00749936, -0.000430178)
  1159. Transformation Matrix:
  1160. 0.999972 0.00092032 0.00745502 0.041356
  1161. -0.000430166 0.997854 -0.0654844 -0.161631
  1162. -0.00749929 0.0654793 0.997826 -0.0470721
  1163. 0 0 0 1
  1164. shift: 0.166838
  1165. -----------------------------------------------------------------
  1166. (Processed/Input): (61 / 61)
  1167. -----------------------------------------------------------------
  1168. Sequence number: 4991
  1169. Number of scan points: 6861 points.
  1170. Number of filtered scan points: 1184 points.
  1171. transformed_scan_ptr: 6861 points.
  1172. map: 6294 points.
  1173. NDT has converged: 1
  1174. Fitness score: 24.7621
  1175. Number of iteration: 8
  1176. (x,y,z,roll,pitch,yaw):
  1177. (-0.00716093, -0.269613, -0.0316585, 0.0745969, 0.00630462, -0.0098581)
  1178. Transformation Matrix:
  1179. 0.999932 0.0103004 0.00555206 -0.00716093
  1180. -0.00985774 0.997166 -0.0745861 -0.269613
  1181. -0.00630458 0.0745263 0.997199 -0.0316585
  1182. 0 0 0 1
  1183. shift: 0.269708
  1184. -----------------------------------------------------------------
  1185. (Processed/Input): (62 / 62)
  1186. -----------------------------------------------------------------
  1187. Sequence number: 4992
  1188. Number of scan points: 6804 points.
  1189. Number of filtered scan points: 1197 points.
  1190. transformed_scan_ptr: 6804 points.
  1191. map: 6294 points.
  1192. NDT has converged: 1
  1193. Fitness score: 25.3983
  1194. Number of iteration: 6
  1195. (x,y,z,roll,pitch,yaw):
  1196. (-0.0128719, -0.107761, 0.0119515, 0.0297058, -0.0295855, -0.00696975)
  1197. Transformation Matrix:
  1198. 0.999538 0.00608805 -0.0297744 -0.0128719
  1199. -0.00696665 0.999541 -0.0294947 -0.107761
  1200. 0.0295812 0.0296885 0.999121 0.0119515
  1201. 0 0 0 1
  1202. shift: 0.108527
  1203. -----------------------------------------------------------------
  1204. (Processed/Input): (63 / 63)
  1205. -----------------------------------------------------------------
  1206. Sequence number: 4993
  1207. Number of scan points: 7248 points.
  1208. Number of filtered scan points: 1267 points.
  1209. transformed_scan_ptr: 7248 points.
  1210. map: 6294 points.
  1211. NDT has converged: 1
  1212. Fitness score: 25.0459
  1213. Number of iteration: 6
  1214. (x,y,z,roll,pitch,yaw):
  1215. (-0.167718, -0.176285, 0.0611739, 0.0438062, -0.0758281, -0.00743549)
  1216. Transformation Matrix:
  1217. 0.997099 0.0041109 -0.0760063 -0.167718
  1218. -0.00741406 0.999038 -0.0432283 -0.176285
  1219. 0.0757554 0.0436664 0.99617 0.0611739
  1220. 0 0 0 1
  1221. shift: 0.243323
  1222. -----------------------------------------------------------------
  1223. (Processed/Input): (64 / 64)
  1224. -----------------------------------------------------------------
  1225. Sequence number: 4994
  1226. Number of scan points: 6974 points.
  1227. Number of filtered scan points: 1256 points.
  1228. transformed_scan_ptr: 6974 points.
  1229. map: 6294 points.
  1230. NDT has converged: 1
  1231. Fitness score: 24.3721
  1232. Number of iteration: 5
  1233. (x,y,z,roll,pitch,yaw):
  1234. (-0.174979, -0.107272, 0.0643426, 0.0468502, -0.0762697, -0.00819251)
  1235. Transformation Matrix:
  1236. 0.997059 0.00461509 -0.0764933 -0.174979
  1237. -0.0081686 0.998898 -0.0462079 -0.107272
  1238. 0.0761958 0.0466969 0.995999 0.0643426
  1239. 0 0 0 1
  1240. shift: 0.205243
  1241. -----------------------------------------------------------------
  1242. (Processed/Input): (65 / 65)
  1243. -----------------------------------------------------------------
  1244. Sequence number: 4995
  1245. Number of scan points: 7123 points.
  1246. Number of filtered scan points: 1257 points.
  1247. transformed_scan_ptr: 7123 points.
  1248. map: 6294 points.
  1249. NDT has converged: 1
  1250. Fitness score: 25.7281
  1251. Number of iteration: 4
  1252. (x,y,z,roll,pitch,yaw):
  1253. (0.0335818, -0.0770933, 0.111906, 0.0159365, 0.00648489, -0.00609739)
  1254. Transformation Matrix:
  1255. 0.99996 0.00619991 0.00638675 0.0335818
  1256. -0.00609723 0.999854 -0.015975 -0.0770933
  1257. -0.00648485 0.0159354 0.999852 0.111906
  1258. 0 0 0 1
  1259. shift: 0.0840899
  1260. -----------------------------------------------------------------
  1261. (Processed/Input): (66 / 66)
  1262. -----------------------------------------------------------------
  1263. Sequence number: 4996
  1264. Number of scan points: 7007 points.
  1265. Number of filtered scan points: 1227 points.
  1266. transformed_scan_ptr: 7007 points.
  1267. map: 6294 points.
  1268. NDT has converged: 1
  1269. Fitness score: 25.5502
  1270. Number of iteration: 6
  1271. (x,y,z,roll,pitch,yaw):
  1272. (0.125799, -0.101069, 0.118909, 0.0432194, 0.0439372, -0.00130048)
  1273. Transformation Matrix:
  1274. 0.999034 0.00319697 0.0438258 0.125799
  1275. -0.00129922 0.999063 -0.0432629 -0.101069
  1276. -0.0439231 0.0431642 0.998102 0.118909
  1277. 0 0 0 1
  1278. shift: 0.16137
  1279. -----------------------------------------------------------------
  1280. (Processed/Input): (67 / 67)
  1281. -----------------------------------------------------------------
  1282. Sequence number: 4997
  1283. Number of scan points: 6943 points.
  1284. Number of filtered scan points: 1242 points.
  1285. transformed_scan_ptr: 6943 points.
  1286. map: 6294 points.
  1287. NDT has converged: 1
  1288. Fitness score: 25.1259
  1289. Number of iteration: 5
  1290. (x,y,z,roll,pitch,yaw):
  1291. (0.0915286, -0.246731, 0.0671197, 0.107555, 0.0349676, -0.0019473)
  1292. Transformation Matrix:
  1293. 0.999387 0.00568897 0.0345493 0.0915286
  1294. -0.00194611 0.994212 -0.107415 -0.246731
  1295. -0.0349605 0.107282 0.993614 0.0671197
  1296. 0 0 0 1
  1297. shift: 0.263161
  1298. -----------------------------------------------------------------
  1299. (Processed/Input): (68 / 68)
  1300. -----------------------------------------------------------------
  1301. Sequence number: 4998
  1302. Number of scan points: 6809 points.
  1303. Number of filtered scan points: 1288 points.
  1304. transformed_scan_ptr: 6809 points.
  1305. map: 6294 points.
  1306. NDT has converged: 1
  1307. Fitness score: 25.4035
  1308. Number of iteration: 4
  1309. (x,y,z,roll,pitch,yaw):
  1310. (0.0626196, -0.34743, 0.0214334, 0.128621, 0.0212147, -0.00223369)
  1311. Transformation Matrix:
  1312. 0.999772 0.00493615 0.0207513 0.0626196
  1313. -0.00223318 0.991731 -0.128313 -0.34743
  1314. -0.0212131 0.128238 0.991517 0.0214334
  1315. 0 0 0 1
  1316. shift: 0.353028
  1317. -----------------------------------------------------------------
  1318. (Processed/Input): (69 / 69)
  1319. -----------------------------------------------------------------
  1320. Sequence number: 4999
  1321. Number of scan points: 6893 points.
  1322. Number of filtered scan points: 1304 points.
  1323. transformed_scan_ptr: 6893 points.
  1324. map: 6294 points.
  1325. NDT has converged: 1
  1326. Fitness score: 25.2005
  1327. Number of iteration: 4
  1328. (x,y,z,roll,pitch,yaw):
  1329. (0.0224305, -0.352227, -0.00741645, 0.122195, 0.0112681, -0.00239096)
  1330. Transformation Matrix:
  1331. 0.999934 0.00374659 0.0108924 0.0224305
  1332. -0.00239081 0.992537 -0.121917 -0.352227
  1333. -0.0112679 0.121883 0.99248 -0.00741645
  1334. 0 0 0 1
  1335. shift: 0.352941
  1336. -----------------------------------------------------------------
  1337. (Processed/Input): (70 / 70)
  1338. -----------------------------------------------------------------
  1339. Sequence number: 5000
  1340. Number of scan points: 6833 points.
  1341. Number of filtered scan points: 1314 points.
  1342. transformed_scan_ptr: 6833 points.
  1343. map: 6294 points.
  1344. NDT has converged: 1
  1345. Fitness score: 25.3025
  1346. Number of iteration: 4
  1347. (x,y,z,roll,pitch,yaw):
  1348. (0.0453446, -0.293043, -0.0236796, 0.108044, 0.00807443, -0.00275272)
  1349. Transformation Matrix:
  1350. 0.999964 0.00360734 0.00773039 0.0453446
  1351. -0.00275262 0.994163 -0.107856 -0.293043
  1352. -0.00807434 0.107831 0.994137 -0.0236796
  1353. 0 0 0 1
  1354. shift: 0.296531
  1355. -----------------------------------------------------------------
  1356. (Processed/Input): (71 / 71)
  1357. -----------------------------------------------------------------
  1358. Sequence number: 5001
  1359. Number of scan points: 6941 points.
  1360. Number of filtered scan points: 1316 points.
  1361. transformed_scan_ptr: 6941 points.
  1362. map: 6294 points.
  1363. NDT has converged: 1
  1364. Fitness score: 25.3806
  1365. Number of iteration: 2
  1366. (x,y,z,roll,pitch,yaw):
  1367. (0.0694761, -0.228277, -0.0280666, 0.0953724, 0.00793961, -0.00472414)
  1368. Transformation Matrix:
  1369. 0.999957 0.0054587 0.00745349 0.0694761
  1370. -0.00472397 0.995441 -0.0952641 -0.228277
  1371. -0.00793952 0.0952249 0.995424 -0.0280666
  1372. 0 0 0 1
  1373. shift: 0.238615
  1374. -----------------------------------------------------------------
  1375. (Processed/Input): (72 / 72)
  1376. -----------------------------------------------------------------
  1377. Sequence number: 5002
  1378. Number of scan points: 7072 points.
  1379. Number of filtered scan points: 1294 points.
  1380. transformed_scan_ptr: 7072 points.
  1381. map: 6294 points.
  1382. NDT has converged: 1
  1383. Fitness score: 25.186
  1384. Number of iteration: 4
  1385. (x,y,z,roll,pitch,yaw):
  1386. (0.021761, -0.251179, -0.0322059, 0.0878234, 0.00781419, -0.00331421)
  1387. Transformation Matrix:
  1388. 0.999964 0.00398681 0.00749326 0.021761
  1389. -0.0033141 0.996138 -0.0877359 -0.251179
  1390. -0.00781411 0.0877079 0.996116 -0.0322059
  1391. 0 0 0 1
  1392. shift: 0.25212
  1393. -----------------------------------------------------------------
  1394. (Processed/Input): (73 / 73)
  1395. -----------------------------------------------------------------
  1396. Sequence number: 5003
  1397. Number of scan points: 7144 points.
  1398. Number of filtered scan points: 1329 points.
  1399. transformed_scan_ptr: 7144 points.
  1400. map: 6294 points.
  1401. NDT has converged: 1
  1402. Fitness score: 25.6537
  1403. Number of iteration: 3
  1404. (x,y,z,roll,pitch,yaw):
  1405. (0.0445624, -0.225607, -0.0341698, 0.0850653, 0.0068585, -0.00133489)
  1406. Transformation Matrix:
  1407. 0.999976 0.00191278 0.00672023 0.0445624
  1408. -0.00133486 0.996382 -0.0849718 -0.225607
  1409. -0.00685845 0.0849608 0.996361 -0.0341698
  1410. 0 0 0 1
  1411. shift: 0.229966
  1412. -----------------------------------------------------------------
  1413. (Processed/Input): (74 / 74)
  1414. -----------------------------------------------------------------
  1415. Sequence number: 5004
  1416. Number of scan points: 7198 points.
  1417. Number of filtered scan points: 1324 points.
  1418. transformed_scan_ptr: 7198 points.
  1419. map: 6294 points.
  1420. NDT has converged: 1
  1421. Fitness score: 25.4446
  1422. Number of iteration: 2
  1423. (x,y,z,roll,pitch,yaw):
  1424. (0.045699, -0.217478, -0.0321544, 0.0835451, 0.00433797, -0.00180756)
  1425. Transformation Matrix:
  1426. 0.999989 0.00216325 0.00417198 0.045699
  1427. -0.00180754 0.99651 -0.0834556 -0.217478
  1428. -0.00433795 0.0834471 0.996503 -0.0321544
  1429. 0 0 0 1
  1430. shift: 0.222227
  1431. -----------------------------------------------------------------
  1432. (Processed/Input): (75 / 75)
  1433. -----------------------------------------------------------------
  1434. Sequence number: 5005
  1435. Number of scan points: 7169 points.
  1436. Number of filtered scan points: 1309 points.
  1437. transformed_scan_ptr: 7169 points.
  1438. map: 6294 points.
  1439. NDT has converged: 1
  1440. Fitness score: 25.1473
  1441. Number of iteration: 2
  1442. (x,y,z,roll,pitch,yaw):
  1443. (0.0139973, -0.189024, -0.0319642, 0.0800322, 0.00127121, -0.000176858)
  1444. Transformation Matrix:
  1445. 0.999999 0.000277922 0.001253 0.0139973
  1446. -0.000176858 0.996799 -0.0799471 -0.189024
  1447. -0.00127121 0.0799468 0.996798 -0.0319642
  1448. 0 0 0 1
  1449. shift: 0.189542
  1450. -----------------------------------------------------------------
  1451. (Processed/Input): (76 / 76)
  1452. -----------------------------------------------------------------
  1453. Sequence number: 5006
  1454. Number of scan points: 7171 points.
  1455. Number of filtered scan points: 1309 points.
  1456. transformed_scan_ptr: 7171 points.
  1457. map: 6294 points.
  1458. NDT has converged: 1
  1459. Fitness score: 25.6113
  1460. Number of iteration: 2
  1461. (x,y,z,roll,pitch,yaw):
  1462. (0.0158754, -0.17725, -0.0315814, 0.0743493, -0.00157327, -0.00322627)
  1463. Transformation Matrix:
  1464. 0.999994 0.00310049 -0.00180857 0.0158754
  1465. -0.00322626 0.997233 -0.0742754 -0.17725
  1466. 0.00157327 0.0742807 0.997236 -0.0315814
  1467. 0 0 0 1
  1468. shift: 0.17796
  1469. -----------------------------------------------------------------
  1470. (Processed/Input): (77 / 77)
  1471. -----------------------------------------------------------------
  1472. Sequence number: 5007
  1473. Number of scan points: 7277 points.
  1474. Number of filtered scan points: 1344 points.
  1475. transformed_scan_ptr: 7277 points.
  1476. map: 6294 points.
  1477. NDT has converged: 1
  1478. Fitness score: 26.0072
  1479. Number of iteration: 3
  1480. (x,y,z,roll,pitch,yaw):
  1481. (0.024833, -0.223288, -0.0290111, 0.0650161, -0.00382537, -0.00362141)
  1482. Transformation Matrix:
  1483. 0.999986 0.00336522 -0.00405254 0.024833
  1484. -0.00362138 0.997882 -0.0649561 -0.223288
  1485. 0.00382536 0.0649698 0.99788 -0.0290111
  1486. 0 0 0 1
  1487. shift: 0.224665
  1488. -----------------------------------------------------------------
  1489. (Processed/Input): (78 / 78)
  1490. -----------------------------------------------------------------
  1491. Sequence number: 5008
  1492. Number of scan points: 7097 points.
  1493. Number of filtered scan points: 1334 points.
  1494. transformed_scan_ptr: 7097 points.
  1495. map: 6294 points.
  1496. NDT has converged: 1
  1497. Fitness score: 26.2816
  1498. Number of iteration: 3
  1499. (x,y,z,roll,pitch,yaw):
  1500. (0.00216084, -0.167606, -0.022939, 0.0549996, -0.00457897, -0.00386086)
  1501. Transformation Matrix:
  1502. 0.999982 0.00360331 -0.00478424 0.00216084
  1503. -0.00386081 0.998481 -0.0549538 -0.167606
  1504. 0.00457896 0.0549713 0.998477 -0.022939
  1505. 0 0 0 1
  1506. shift: 0.16762
  1507. -----------------------------------------------------------------
  1508. (Processed/Input): (79 / 79)
  1509. -----------------------------------------------------------------
  1510. Sequence number: 5009
  1511. Number of scan points: 7185 points.
  1512. Number of filtered scan points: 1322 points.
  1513. transformed_scan_ptr: 7185 points.
  1514. map: 6294 points.
  1515. NDT has converged: 1
  1516. Fitness score: 26.5792
  1517. Number of iteration: 2
  1518. (x,y,z,roll,pitch,yaw):
  1519. (0.0142493, -0.116603, -0.0193155, 0.0425342, -0.00357364, -0.00210435)
  1520. Transformation Matrix:
  1521. 0.999991 0.00195049 -0.00365988 0.0142493
  1522. -0.00210434 0.999094 -0.0425138 -0.116603
  1523. 0.00357363 0.0425211 0.999089 -0.0193155
  1524. 0 0 0 1
  1525. shift: 0.117471
  1526. -----------------------------------------------------------------
  1527. (Processed/Input): (80 / 80)
  1528. -----------------------------------------------------------------
  1529. Sequence number: 5010
  1530. Number of scan points: 7220 points.
  1531. Number of filtered scan points: 1344 points.
  1532. transformed_scan_ptr: 7220 points.
  1533. map: 6294 points.
  1534. NDT has converged: 1
  1535. Fitness score: 26.7013
  1536. Number of iteration: 3
  1537. (x,y,z,roll,pitch,yaw):
  1538. (0.0217211, -0.0786046, -0.0141213, 0.0242126, -0.00270576, -0.0039994)
  1539. Transformation Matrix:
  1540. 0.999988 0.00393271 -0.00280177 0.0217211
  1541. -0.00399937 0.999699 -0.0241992 -0.0786046
  1542. 0.00270576 0.0242101 0.999703 -0.0141213
  1543. 0 0 0 1
  1544. shift: 0.0815505
  1545. -----------------------------------------------------------------
  1546. (Processed/Input): (81 / 81)
  1547. -----------------------------------------------------------------
  1548. Sequence number: 5011
  1549. Number of scan points: 7285 points.
  1550. Number of filtered scan points: 1335 points.
  1551. transformed_scan_ptr: 7285 points.
  1552. map: 6294 points.
  1553. NDT has converged: 1
  1554. Fitness score: 26.5794
  1555. Number of iteration: 3
  1556. (x,y,z,roll,pitch,yaw):
  1557. (0.0117311, -0.0798609, -0.0127032, 0.0144534, -0.00276512, -0.00294879)
  1558. Transformation Matrix:
  1559. 0.999992 0.00290853 -0.00280743 0.0117311
  1560. -0.00294878 0.999891 -0.0144447 -0.0798609
  1561. 0.00276512 0.0144528 0.999892 -0.0127032
  1562. 0 0 0 1
  1563. shift: 0.0807179
  1564. -----------------------------------------------------------------
  1565. (Processed/Input): (82 / 82)
  1566. -----------------------------------------------------------------
  1567. Sequence number: 5012
  1568. Number of scan points: 7177 points.
  1569. Number of filtered scan points: 1350 points.
  1570. transformed_scan_ptr: 7177 points.
  1571. map: 6294 points.
  1572. NDT has converged: 1
  1573. Fitness score: 26.8153
  1574. Number of iteration: 2
  1575. (x,y,z,roll,pitch,yaw):
  1576. (0.0212812, -0.0521394, -0.0120538, 0.00667716, -0.0025992, -0.0057494)
  1577. Transformation Matrix:
  1578. 0.99998 0.00573189 -0.00263748 0.0212812
  1579. -0.00574935 0.999961 -0.00666206 -0.0521394
  1580. 0.0025992 0.00667709 0.999974 -0.0120538
  1581. 0 0 0 1
  1582. shift: 0.0563153
  1583. -----------------------------------------------------------------
  1584. (Processed/Input): (83 / 83)
  1585. -----------------------------------------------------------------
  1586. Sequence number: 5013
  1587. Number of scan points: 7215 points.
  1588. Number of filtered scan points: 1339 points.
  1589. transformed_scan_ptr: 7215 points.
  1590. map: 6294 points.
  1591. NDT has converged: 1
  1592. Fitness score: 26.8248
  1593. Number of iteration: 2
  1594. (x,y,z,roll,pitch,yaw):
  1595. (0.0234503, -0.00524644, -0.0144821, 0.000218835, -0.00155633, -0.00156327)
  1596. Transformation Matrix:
  1597. 0.999998 0.00156293 -0.00155667 0.0234503
  1598. -0.00156326 0.999999 -0.000216415 -0.00524644
  1599. 0.00155633 0.000218835 0.999999 -0.0144821
  1600. 0 0 0 1
  1601. shift: 0.02403
  1602. -----------------------------------------------------------------
  1603. (Processed/Input): (84 / 84)
  1604. -----------------------------------------------------------------
  1605. Sequence number: 5014
  1606. Number of scan points: 7267 points.
  1607. Number of filtered scan points: 1332 points.
  1608. transformed_scan_ptr: 7267 points.
  1609. map: 6294 points.
  1610. NDT has converged: 1
  1611. Fitness score: 26.5995
  1612. Number of iteration: 2
  1613. (x,y,z,roll,pitch,yaw):
  1614. (0.0355194, -0.0117753, -0.0174698, -0.00144651, -0.000750538, 0.000134567)
  1615. Transformation Matrix:
  1616. 1 -0.000133502 -0.000750728 0.0355194
  1617. 0.000134567 0.999999 0.00144641 -0.0117753
  1618. 0.000750538 -0.00144651 0.999999 -0.0174698
  1619. 0 0 0 1
  1620. shift: 0.0374204
  1621. -----------------------------------------------------------------
  1622. (Processed/Input): (85 / 85)
  1623. -----------------------------------------------------------------
  1624. Sequence number: 5015
  1625. Number of scan points: 7162 points.
  1626. Number of filtered scan points: 1332 points.
  1627. transformed_scan_ptr: 7162 points.
  1628. map: 6294 points.
  1629. NDT has converged: 1
  1630. Fitness score: 27.0915
  1631. Number of iteration: 2
  1632. (x,y,z,roll,pitch,yaw):
  1633. (0.0336162, -0.0285909, -0.0249187, -0.00150705, -0.00105962, -0.000594787)
  1634. Transformation Matrix:
  1635. 0.999999 0.000596361 -0.00105874 0.0336162
  1636. -0.000594787 0.999999 0.00150767 -0.0285909
  1637. 0.00105962 -0.00150704 0.999998 -0.0249187
  1638. 0 0 0 1
  1639. shift: 0.0441303
  1640. -----------------------------------------------------------------
  1641. (Processed/Input): (86 / 86)
  1642. -----------------------------------------------------------------
  1643. Sequence number: 5016
  1644. Number of scan points: 7186 points.
  1645. Number of filtered scan points: 1331 points.
  1646. transformed_scan_ptr: 7186 points.
  1647. map: 6294 points.
  1648. NDT has converged: 1
  1649. Fitness score: 27.0589
  1650. Number of iteration: 2
  1651. (x,y,z,roll,pitch,yaw):
  1652. (0.0436718, -0.0464959, -0.0311566, 0.00117319, -0.00134243, -0.000421815)
  1653. Transformation Matrix:
  1654. 0.999999 0.000420248 -0.00134292 0.0436718
  1655. -0.000421815 0.999999 -0.00117263 -0.0464959
  1656. 0.00134243 0.00117319 0.999998 -0.0311566
  1657. 0 0 0 1
  1658. shift: 0.0637894
  1659. -----------------------------------------------------------------
  1660. (Processed/Input): (87 / 87)
  1661. -----------------------------------------------------------------
  1662. Sequence number: 5017
  1663. Number of scan points: 7269 points.
  1664. Number of filtered scan points: 1340 points.
  1665. transformed_scan_ptr: 7269 points.
  1666. map: 6294 points.
  1667. NDT has converged: 1
  1668. Fitness score: 27.3447
  1669. Number of iteration: 2
  1670. (x,y,z,roll,pitch,yaw):
  1671. (0.0394017, -0.0464102, -0.0351532, 0.0032901, -0.00144364, -0.00167024)
  1672. Transformation Matrix:
  1673. 0.999998 0.00166547 -0.00144914 0.0394017
  1674. -0.00167024 0.999993 -0.00328767 -0.0464102
  1675. 0.00144364 0.00329009 0.999994 -0.0351532
  1676. 0 0 0 1
  1677. shift: 0.0608802
  1678. -----------------------------------------------------------------
  1679. (Processed/Input): (88 / 88)
  1680. -----------------------------------------------------------------
  1681. Sequence number: 5018
  1682. Number of scan points: 7266 points.
  1683. Number of filtered scan points: 1337 points.
  1684. transformed_scan_ptr: 7266 points.
  1685. map: 6294 points.
  1686. NDT has converged: 1
  1687. Fitness score: 27.4681
  1688. Number of iteration: 2
  1689. (x,y,z,roll,pitch,yaw):
  1690. (0.0487053, -0.0392284, -0.0437709, 0.00345524, -0.00137476, -0.00188965)
  1691. Transformation Matrix:
  1692. 0.999997 0.00188489 -0.00138128 0.0487053
  1693. -0.00188965 0.999992 -0.00345263 -0.0392284
  1694. 0.00137476 0.00345523 0.999993 -0.0437709
  1695. 0 0 0 1
  1696. shift: 0.0625386
  1697. -----------------------------------------------------------------
  1698. (Processed/Input): (89 / 89)
  1699. -----------------------------------------------------------------
  1700. Sequence number: 5019
  1701. Number of scan points: 7151 points.
  1702. Number of filtered scan points: 1322 points.
  1703. transformed_scan_ptr: 7151 points.
  1704. map: 6294 points.
  1705. NDT has converged: 1
  1706. Fitness score: 27.4554
  1707. Number of iteration: 2
  1708. (x,y,z,roll,pitch,yaw):
  1709. (0.0303628, -0.0156007, -0.0513662, -0.00148694, -0.00153602, -0.00053624)
  1710. Transformation Matrix:
  1711. 0.999999 0.000538537 -0.00153522 0.0303628
  1712. -0.000536239 0.999999 0.00148776 -0.0156007
  1713. 0.00153602 -0.00148693 0.999998 -0.0513662
  1714. 0 0 0 1
  1715. shift: 0.0341362
  1716. -----------------------------------------------------------------
  1717. (Processed/Input): (90 / 90)
  1718. -----------------------------------------------------------------
  1719. Sequence number: 5020
  1720. Number of scan points: 7102 points.
  1721. Number of filtered scan points: 1313 points.
  1722. transformed_scan_ptr: 7102 points.
  1723. map: 6294 points.
  1724. NDT has converged: 1
  1725. Fitness score: 27.3591
  1726. Number of iteration: 3
  1727. (x,y,z,roll,pitch,yaw):
  1728. (0.0058644, -0.0066732, -0.0559401, -0.0121747, -0.00128827, -0.00476505)
  1729. Transformation Matrix:
  1730. 0.999988 0.00478036 -0.00123016 0.0058644
  1731. -0.00476503 0.999914 0.0121804 -0.0066732
  1732. 0.00128827 -0.0121744 0.999925 -0.0559401
  1733. 0 0 0 1
  1734. shift: 0.00888385
  1735. -----------------------------------------------------------------
  1736. (Processed/Input): (91 / 91)
  1737. -----------------------------------------------------------------
  1738. Sequence number: 5021
  1739. Number of scan points: 7115 points.
  1740. Number of filtered scan points: 1325 points.
  1741. transformed_scan_ptr: 7115 points.
  1742. map: 6294 points.
  1743. NDT has converged: 1
  1744. Fitness score: 27.199
  1745. Number of iteration: 2
  1746. (x,y,z,roll,pitch,yaw):
  1747. (0.00825556, 0.00467373, -0.0596311, -0.0277242, -0.00248579, -0.00169857)
  1748. Transformation Matrix:
  1749. 0.999995 0.00176683 -0.00243774 0.00825556
  1750. -0.00169856 0.999614 0.0277248 0.00467373
  1751. 0.00248579 -0.0277206 0.999613 -0.0596311
  1752. 0 0 0 1
  1753. shift: 0.00948673
  1754. -----------------------------------------------------------------
  1755. (Processed/Input): (92 / 92)
  1756. -----------------------------------------------------------------
  1757. Sequence number: 5022
  1758. Number of scan points: 7143 points.
  1759. Number of filtered scan points: 1319 points.
  1760. transformed_scan_ptr: 7143 points.
  1761. map: 6294 points.
  1762. NDT has converged: 1
  1763. Fitness score: 26.7587
  1764. Number of iteration: 3
  1765. (x,y,z,roll,pitch,yaw):
  1766. (0.0249613, 0.0594188, -0.0651992, -0.0466628, -0.00426436, -0.00493584)
  1767. Transformation Matrix:
  1768. 0.999979 0.00512936 -0.00402942 0.0249613
  1769. -0.00493578 0.998898 0.0466664 0.0594188
  1770. 0.00426434 -0.0466455 0.998902 -0.0651992
  1771. 0 0 0 1
  1772. shift: 0.0644489
  1773. -----------------------------------------------------------------
  1774. (Processed/Input): (93 / 93)
  1775. -----------------------------------------------------------------
  1776. Sequence number: 5023
  1777. Number of scan points: 6947 points.
  1778. Number of filtered scan points: 1322 points.
  1779. transformed_scan_ptr: 6947 points.
  1780. map: 6294 points.
  1781. NDT has converged: 1
  1782. Fitness score: 26.2299
  1783. Number of iteration: 3
  1784. (x,y,z,roll,pitch,yaw):
  1785. (-0.00290237, 0.111845, -0.0585938, -0.0622268, -0.00706523, -6.42067e-05)
  1786. Transformation Matrix:
  1787. 0.999975 0.000503438 -0.00704751 -0.00290237
  1788. -6.42051e-05 0.998065 0.0621871 0.111845
  1789. 0.00706517 -0.0621851 0.99804 -0.0585938
  1790. 0 0 0 1
  1791. shift: 0.111883
  1792. -----------------------------------------------------------------
  1793. (Processed/Input): (94 / 94)
  1794. -----------------------------------------------------------------
  1795. Sequence number: 5024
  1796. Number of scan points: 7066 points.
  1797. Number of filtered scan points: 1374 points.
  1798. transformed_scan_ptr: 7066 points.
  1799. map: 6294 points.
  1800. NDT has converged: 1
  1801. Fitness score: 26.4287
  1802. Number of iteration: 2
  1803. (x,y,z,roll,pitch,yaw):
  1804. (-0.0447949, 0.151675, -0.0498557, -0.077091, -0.00974932, -0.00284288)
  1805. Transformation Matrix:
  1806. 0.999948 0.00358526 -0.00950123 -0.0447949
  1807. -0.00284274 0.997024 0.077042 0.151675
  1808. 0.00974916 -0.077011 0.996983 -0.0498557
  1809. 0 0 0 1
  1810. shift: 0.158151
  1811. -----------------------------------------------------------------
  1812. (Processed/Input): (95 / 95)
  1813. -----------------------------------------------------------------
  1814. Sequence number: 5025
  1815. Number of scan points: 7036 points.
  1816. Number of filtered scan points: 1351 points.
  1817. transformed_scan_ptr: 7036 points.
  1818. map: 6294 points.
  1819. NDT has converged: 1
  1820. Fitness score: 25.9988
  1821. Number of iteration: 3
  1822. (x,y,z,roll,pitch,yaw):
  1823. (-0.0508213, 0.117942, -0.0365139, -0.0809699, -0.0106748, -0.00355391)
  1824. Transformation Matrix:
  1825. 0.999937 0.00440564 -0.0103521 -0.0508213
  1826. -0.0035537 0.996714 0.0809188 0.117942
  1827. 0.0106746 -0.0808768 0.996667 -0.0365139
  1828. 0 0 0 1
  1829. shift: 0.128425
  1830. -----------------------------------------------------------------
  1831. (Processed/Input): (96 / 96)
  1832. -----------------------------------------------------------------
  1833. Sequence number: 5026
  1834. Number of scan points: 7241 points.
  1835. Number of filtered scan points: 1380 points.
  1836. transformed_scan_ptr: 7241 points.
  1837. map: 6294 points.
  1838. NDT has converged: 1
  1839. Fitness score: 26.0505
  1840. Number of iteration: 3
  1841. (x,y,z,roll,pitch,yaw):
  1842. (-0.00313494, 0.13825, -0.0235561, -0.0757679, -0.00853554, -0.00236601)
  1843. Transformation Matrix:
  1844. 0.999961 0.00300531 -0.00833183 -0.00313494
  1845. -0.00236592 0.997127 0.0757154 0.13825
  1846. 0.00853544 -0.0756927 0.997095 -0.0235561
  1847. 0 0 0 1
  1848. shift: 0.138286
  1849. -----------------------------------------------------------------
  1850. (Processed/Input): (97 / 97)
  1851. -----------------------------------------------------------------
  1852. Sequence number: 5027
  1853. Number of scan points: 7178 points.
  1854. Number of filtered scan points: 1385 points.
  1855. transformed_scan_ptr: 7178 points.
  1856. map: 6294 points.
  1857. NDT has converged: 1
  1858. Fitness score: 25.6863
  1859. Number of iteration: 3
  1860. (x,y,z,roll,pitch,yaw):
  1861. (0.0325517, 0.0990408, -0.0083144, -0.0611661, -0.0062466, -0.00175301)
  1862. Transformation Matrix:
  1863. 0.999979 0.00213157 -0.00612771 0.0325517
  1864. -0.00175298 0.998128 0.0611388 0.0990408
  1865. 0.00624656 -0.0611268 0.99811 -0.0083144
  1866. 0 0 0 1
  1867. shift: 0.104253
  1868. -----------------------------------------------------------------
  1869. (Processed/Input): (98 / 98)
  1870. (Processed/Input): (99 / 99)
  1871. -----------------------------------------------------------------
  1872. Sequence number: 5028
  1873. Number of scan points: 7238 points.
  1874. Number of filtered scan points: 1308 points.
  1875. transformed_scan_ptr: 7238 points.
  1876. map: 6294 points.
  1877. NDT has converged: 1
  1878. Fitness score: 25.6463
  1879. Number of iteration: 2
  1880. (x,y,z,roll,pitch,yaw):
  1881. (0.0406336, 0.0710913, 0.000544692, -0.0471956, -0.00129338, -0.00570591)
  1882. Transformation Matrix:
  1883. 0.999983 0.00576055 -0.00102273 0.0406336
  1884. -0.00570588 0.99887 0.0471847 0.0710913
  1885. 0.00129338 -0.0471781 0.998886 0.000544692
  1886. 0 0 0 1
  1887. shift: 0.0818844
  1888. -----------------------------------------------------------------
  1889. -----------------------------------------------------------------
  1890. Sequence number: 5029
  1891. Number of scan points: 7162 points.
  1892. Number of filtered scan points: 1304 points.
  1893. transformed_scan_ptr: 7162 points.
  1894. map: 6294 points.
  1895. NDT has converged: 1
  1896. Fitness score: 26.027
  1897. Number of iteration: 3
  1898. (x,y,z,roll,pitch,yaw):
  1899. (0.0297307, 0.0673409, 0.00444536, -0.0387459, 0.00308806, -0.00481902)
  1900. Transformation Matrix:
  1901. 0.999984 0.00469576 0.00327238 0.0297307
  1902. -0.00481898 0.999238 0.0387209 0.0673409
  1903. -0.00308806 -0.038736 0.999245 0.00444536
  1904. 0 0 0 1
  1905. shift: 0.0736119
  1906. -----------------------------------------------------------------
  1907. (Processed/Input): (100 / 100)
  1908. -----------------------------------------------------------------
  1909. Sequence number: 5030
  1910. Number of scan points: 7098 points.
  1911. Number of filtered scan points: 1299 points.
  1912. transformed_scan_ptr: 7098 points.
  1913. map: 6294 points.
  1914. NDT has converged: 1
  1915. Fitness score: 26.314
  1916. Number of iteration: 2
  1917. (x,y,z,roll,pitch,yaw):
  1918. (0.0347806, 0.0428292, 0.00175324, -0.0386025, 0.00584424, -0.005206)
  1919. Transformation Matrix:
  1920. 0.999969 0.00497655 0.00604069 0.0347806
  1921. -0.00520589 0.999243 0.038562 0.0428292
  1922. -0.0058442 -0.0385922 0.999238 0.00175324
  1923. 0 0 0 1
  1924. shift: 0.0551728
  1925. -----------------------------------------------------------------
  1926. (Processed/Input): (101 / 101)
  1927. -----------------------------------------------------------------
  1928. Sequence number: 5031
  1929. Number of scan points: 7107 points.
  1930. Number of filtered scan points: 1299 points.
  1931. transformed_scan_ptr: 7107 points.
  1932. map: 6294 points.
  1933. NDT has converged: 1
  1934. Fitness score: 26.0196
  1935. Number of iteration: 2
  1936. (x,y,z,roll,pitch,yaw):
  1937. (0.0520548, 0.037951, -0.00449463, -0.0462126, 0.00774637, -0.0095279)
  1938. Transformation Matrix:
  1939. 0.999925 0.00915976 0.00817782 0.0520548
  1940. -0.00952747 0.99889 0.0461203 0.037951
  1941. -0.00774629 -0.0461947 0.998902 -0.00449463
  1942. 0 0 0 1
  1943. shift: 0.0644203
  1944. -----------------------------------------------------------------
  1945. (Processed/Input): (102 / 102)
  1946. -----------------------------------------------------------------
  1947. Sequence number: 5032
  1948. Number of scan points: 7156 points.
  1949. Number of filtered scan points: 1333 points.
  1950. transformed_scan_ptr: 7156 points.
  1951. map: 6294 points.
  1952. NDT has converged: 1
  1953. Fitness score: 26.0203
  1954. Number of iteration: 2
  1955. (x,y,z,roll,pitch,yaw):
  1956. (0.0658653, 0.0587338, -0.0162472, -0.0558933, 0.00801693, -0.00802672)
  1957. Transformation Matrix:
  1958. 0.999936 0.00756626 0.00845246 0.0658653
  1959. -0.00802637 0.99841 0.0557982 0.0587338
  1960. -0.00801684 -0.0558625 0.998406 -0.0162472
  1961. 0 0 0 1
  1962. shift: 0.088249
  1963. -----------------------------------------------------------------
  1964. (Processed/Input): (103 / 103)
  1965. -----------------------------------------------------------------
  1966. Sequence number: 5033
  1967. Number of scan points: 7179 points.
  1968. Number of filtered scan points: 1329 points.
  1969. transformed_scan_ptr: 7179 points.
  1970. map: 6294 points.
  1971. NDT has converged: 1
  1972. Fitness score: 25.8256
  1973. Number of iteration: 3
  1974. (x,y,z,roll,pitch,yaw):
  1975. (0.0346017, 0.121714, -0.0184858, -0.0693123, 0.00517053, -0.00371475)
  1976. Transformation Matrix:
  1977. 0.99998 0.00334773 0.00541533 0.0346017
  1978. -0.00371469 0.997593 0.0692372 0.121714
  1979. -0.00517051 -0.0692559 0.997586 -0.0184858
  1980. 0 0 0 1
  1981. shift: 0.126537
  1982. -----------------------------------------------------------------
  1983. (Processed/Input): (104 / 104)
  1984. -----------------------------------------------------------------
  1985. Sequence number: 5034
  1986. Number of scan points: 6943 points.
  1987. Number of filtered scan points: 1318 points.
  1988. transformed_scan_ptr: 6943 points.
  1989. map: 6294 points.
  1990. NDT has converged: 1
  1991. Fitness score: 26.1471
  1992. Number of iteration: 4
  1993. (x,y,z,roll,pitch,yaw):
  1994. (0.0247091, 0.156226, -0.0129416, -0.0843474, 0.00107793, -0.000935366)
  1995. Transformation Matrix:
  1996. 0.999999 0.000841227 0.0011529 0.0247091
  1997. -0.000935366 0.996444 0.0842464 0.156226
  1998. -0.00107793 -0.0842474 0.996444 -0.0129416
  1999. 0 0 0 1
  2000. shift: 0.158168
  2001. -----------------------------------------------------------------
  2002. (Processed/Input): (105 / 105)
  2003. -----------------------------------------------------------------
  2004. Sequence number: 5035
  2005. Number of scan points: 7021 points.
  2006. Number of filtered scan points: 1288 points.
  2007. transformed_scan_ptr: 7021 points.
  2008. map: 6294 points.
  2009. NDT has converged: 1
  2010. Fitness score: 25.0749
  2011. Number of iteration: 4
  2012. (x,y,z,roll,pitch,yaw):
  2013. (0.00970651, 0.156048, -0.0062631, -0.092238, -0.00159972, -0.0045137)
  2014. Transformation Matrix:
  2015. 0.999989 0.00464184 -0.00117716 0.00970651
  2016. -0.00451368 0.995738 0.0921135 0.156048
  2017. 0.00159972 -0.0921071 0.995748 -0.0062631
  2018. 0 0 0 1
  2019. shift: 0.156349
  2020. -----------------------------------------------------------------
  2021. (Processed/Input): (106 / 106)
  2022. -----------------------------------------------------------------
  2023. Sequence number: 5036
  2024. Number of scan points: 7087 points.
  2025. Number of filtered scan points: 1359 points.
  2026. transformed_scan_ptr: 7087 points.
  2027. map: 6294 points.
  2028. NDT has converged: 1
  2029. Fitness score: 25.4661
  2030. Number of iteration: 3
  2031. (x,y,z,roll,pitch,yaw):
  2032. (-0.0105241, 0.239044, 0.0128923, -0.118965, -0.0190743, -0.00318948)
  2033. Transformation Matrix:
  2034. 0.999813 0.0054306 -0.0185597 -0.0105241
  2035. -0.00318889 0.99292 0.118744 0.239044
  2036. 0.0190732 -0.118663 0.992751 0.0128923
  2037. 0 0 0 1
  2038. shift: 0.239275
  2039. -----------------------------------------------------------------
  2040. (Processed/Input): (107 / 107)
  2041. -----------------------------------------------------------------
  2042. Sequence number: 5037
  2043. Number of scan points: 7148 points.
  2044. Number of filtered scan points: 1378 points.
  2045. transformed_scan_ptr: 7148 points.
  2046. map: 6294 points.
  2047. NDT has converged: 1
  2048. Fitness score: 25.4261
  2049. Number of iteration: 3
  2050. (x,y,z,roll,pitch,yaw):
  2051. (-0.087753, 0.22428, 0.0532115, -0.0986031, -0.0437387, -0.000510655)
  2052. Transformation Matrix:
  2053. 0.999043 0.00481259 -0.0434621 -0.087753
  2054. -0.000510167 0.99514 0.0984657 0.22428
  2055. 0.0437247 -0.0983493 0.994191 0.0532115
  2056. 0 0 0 1
  2057. shift: 0.240837
  2058. -----------------------------------------------------------------
  2059. (Processed/Input): (108 / 108)
  2060. -----------------------------------------------------------------
  2061. Sequence number: 5038
  2062. Number of scan points: 7486 points.
  2063. Number of filtered scan points: 1350 points.
  2064. transformed_scan_ptr: 7486 points.
  2065. map: 6294 points.
  2066. NDT has converged: 1
  2067. Fitness score: 25.553
  2068. Number of iteration: 3
  2069. (x,y,z,roll,pitch,yaw):
  2070. (-0.0638638, 0.236441, 0.0566867, -0.0974776, -0.0356233, -0.00254898)
  2071. Transformation Matrix:
  2072. 0.999362 0.00600311 -0.0351985 -0.0638638
  2073. -0.00254736 0.995241 0.0974133 0.236441
  2074. 0.0356157 -0.0972616 0.994621 0.0566867
  2075. 0 0 0 1
  2076. shift: 0.244915
  2077. -----------------------------------------------------------------
  2078. (Processed/Input): (109 / 109)
  2079. -----------------------------------------------------------------
  2080. Sequence number: 5039
  2081. Number of scan points: 7162 points.
  2082. Number of filtered scan points: 1334 points.
  2083. transformed_scan_ptr: 7162 points.
  2084. map: 6294 points.
  2085. NDT has converged: 1
  2086. Fitness score: 25.177
  2087. Number of iteration: 3
  2088. (x,y,z,roll,pitch,yaw):
  2089. (0.0728958, 0.144014, 0.0446987, -0.0653509, 0.0120427, -0.00295255)
  2090. Transformation Matrix:
  2091. 0.999923 0.00215982 0.0122094 0.0728958
  2092. -0.00295233 0.997863 0.0652686 0.144014
  2093. -0.0120424 -0.0652997 0.997793 0.0446987
  2094. 0 0 0 1
  2095. shift: 0.161412
  2096. -----------------------------------------------------------------
  2097. (Processed/Input): (110 / 110)
  2098. -----------------------------------------------------------------
  2099. Sequence number: 5040
  2100. Number of scan points: 7214 points.
  2101. Number of filtered scan points: 1260 points.
  2102. transformed_scan_ptr: 7214 points.
  2103. map: 6294 points.
  2104. NDT has converged: 1
  2105. Fitness score: 25.3922
  2106. Number of iteration: 4
  2107. (x,y,z,roll,pitch,yaw):
  2108. (0.141429, 0.127669, 0.0522555, -0.0624499, 0.0464664, -0.00470368)
  2109. Transformation Matrix:
  2110. 0.99891 0.00179563 0.0466522 0.141429
  2111. -0.00469859 0.998053 0.0621906 0.127669
  2112. -0.0464497 -0.062342 0.996973 0.0522555
  2113. 0 0 0 1
  2114. shift: 0.19053
  2115. -----------------------------------------------------------------
  2116. (Processed/Input): (111 / 111)
  2117. -----------------------------------------------------------------
  2118. Sequence number: 5041
  2119. Number of scan points: 7135 points.
  2120. Number of filtered scan points: 1307 points.
  2121. transformed_scan_ptr: 7135 points.
  2122. map: 6294 points.
  2123. NDT has converged: 1
  2124. Fitness score: 25.0538
  2125. Number of iteration: 3
  2126. (x,y,z,roll,pitch,yaw):
  2127. (0.171025, 0.183441, 0.0459838, -0.0880297, 0.0552914, -0.00758553)
  2128. Transformation Matrix:
  2129. 0.998443 0.00269772 0.0557145 0.171025
  2130. -0.00757386 0.996136 0.087496 0.183441
  2131. -0.0552632 -0.0877817 0.994606 0.0459838
  2132. 0 0 0 1
  2133. shift: 0.250799
  2134. -----------------------------------------------------------------
  2135. (Processed/Input): (112 / 112)
  2136. -----------------------------------------------------------------
  2137. Sequence number: 5042
  2138. Number of scan points: 7021 points.
  2139. Number of filtered scan points: 1299 points.
  2140. transformed_scan_ptr: 7021 points.
  2141. map: 6294 points.
  2142. NDT has converged: 1
  2143. Fitness score: 24.376
  2144. Number of iteration: 5
  2145. (x,y,z,roll,pitch,yaw):
  2146. (0.0544326, 0.257448, 0.00823661, -0.119426, 0.0338982, -0.00510256)
  2147. Transformation Matrix:
  2148. 0.999412 0.00102832 0.0342578 0.0544326
  2149. -0.00509961 0.992885 0.118969 0.257448
  2150. -0.0338917 -0.119073 0.992307 0.00823661
  2151. 0 0 0 1
  2152. shift: 0.263139
  2153. -----------------------------------------------------------------
  2154. (Processed/Input): (113 / 113)
  2155. -----------------------------------------------------------------
  2156. Sequence number: 5043
  2157. Number of scan points: 6962 points.
  2158. Number of filtered scan points: 1297 points.
  2159. transformed_scan_ptr: 6962 points.
  2160. map: 6294 points.
  2161. NDT has converged: 1
  2162. Fitness score: 25.0076
  2163. Number of iteration: 4
  2164. (x,y,z,roll,pitch,yaw):
  2165. (0.0788897, 0.208983, -0.0160042, -0.103391, 0.0167502, -0.00102167)
  2166. Transformation Matrix:
  2167. 0.999859 -0.000712449 0.0167654 0.0788897
  2168. -0.00102152 0.994661 0.10319 0.208983
  2169. -0.0167494 -0.103192 0.99452 -0.0160042
  2170. 0 0 0 1
  2171. shift: 0.223377
  2172. -----------------------------------------------------------------
  2173. (Processed/Input): (114 / 114)
  2174. -----------------------------------------------------------------
  2175. Sequence number: 5044
  2176. Number of scan points: 7036 points.
  2177. Number of filtered scan points: 1287 points.
  2178. transformed_scan_ptr: 7036 points.
  2179. map: 6294 points.
  2180. NDT has converged: 1
  2181. Fitness score: 25.4816
  2182. Number of iteration: 3
  2183. (x,y,z,roll,pitch,yaw):
  2184. (0.0521499, 0.162203, -0.0274677, -0.0865956, 0.0126607, -0.00329399)
  2185. Transformation Matrix:
  2186. 0.999914 0.00218668 0.0128978 0.0521499
  2187. -0.00329372 0.996251 0.0864454 0.162203
  2188. -0.0126604 -0.0864805 0.996173 -0.0274677
  2189. 0 0 0 1
  2190. shift: 0.170381
  2191. -----------------------------------------------------------------
  2192. (Processed/Input): (115 / 115)
  2193. -----------------------------------------------------------------
  2194. Sequence number: 5045
  2195. Number of scan points: 7048 points.
  2196. Number of filtered scan points: 1301 points.
  2197. transformed_scan_ptr: 7048 points.
  2198. map: 6294 points.
  2199. NDT has converged: 1
  2200. Fitness score: 26.1778
  2201. Number of iteration: 3
  2202. (x,y,z,roll,pitch,yaw):
  2203. (0.0603184, 0.139445, -0.0262469, -0.075938, 0.0108267, -0.00972072)
  2204. Transformation Matrix:
  2205. 0.999894 0.00887123 0.0115322 0.0603184
  2206. -0.00971999 0.997079 0.0757565 0.139445
  2207. -0.0108265 -0.0758606 0.99706 -0.0262469
  2208. 0 0 0 1
  2209. shift: 0.151932
  2210. -----------------------------------------------------------------
  2211. (Processed/Input): (116 / 116)
  2212. -----------------------------------------------------------------
  2213. Sequence number: 5046
  2214. Number of scan points: 7113 points.
  2215. Number of filtered scan points: 1300 points.
  2216. transformed_scan_ptr: 7113 points.
  2217. map: 6294 points.
  2218. NDT has converged: 1
  2219. Fitness score: 25.5113
  2220. Number of iteration: 2
  2221. (x,y,z,roll,pitch,yaw):
  2222. (0.0543637, 0.101657, -0.0355411, -0.0683677, 0.00794078, -0.00445222)
  2223. Transformation Matrix:
  2224. 0.999959 0.00389935 0.00822621 0.0543637
  2225. -0.00445206 0.997656 0.0682785 0.101657
  2226. -0.00794069 -0.0683123 0.997632 -0.0355411
  2227. 0 0 0 1
  2228. shift: 0.115281
  2229. -----------------------------------------------------------------
  2230. (Processed/Input): (117 / 117)
  2231. -----------------------------------------------------------------
  2232. Sequence number: 5047
  2233. Number of scan points: 7082 points.
  2234. Number of filtered scan points: 1239 points.
  2235. transformed_scan_ptr: 7082 points.
  2236. map: 6294 points.
  2237. NDT has converged: 1
  2238. Fitness score: 25.3354
  2239. Number of iteration: 5
  2240. (x,y,z,roll,pitch,yaw):
  2241. (0.0357579, 0.159898, -0.0320713, -0.061768, 0.00315277, 0.00144383)
  2242. Transformation Matrix:
  2243. 0.999994 -0.00163569 0.00305763 0.0357579
  2244. 0.00144382 0.998092 0.0617332 0.159898
  2245. -0.00315277 -0.0617284 0.998088 -0.0320713
  2246. 0 0 0 1
  2247. shift: 0.163847
  2248. -----------------------------------------------------------------
  2249. (Processed/Input): (118 / 118)
  2250. -----------------------------------------------------------------
  2251. Sequence number: 5048
  2252. Number of scan points: 7129 points.
  2253. Number of filtered scan points: 1234 points.
  2254. transformed_scan_ptr: 7129 points.
  2255. map: 6294 points.
  2256. NDT has converged: 1
  2257. Fitness score: 25.1935
  2258. Number of iteration: 4
  2259. (x,y,z,roll,pitch,yaw):
  2260. (0.0424806, 0.158281, -0.0306162, -0.0569944, -0.000325097, -0.00144805)
  2261. Transformation Matrix:
  2262. 0.999999 0.00146422 -0.000242082 0.0424806
  2263. -0.00144805 0.998375 0.056964 0.158281
  2264. 0.000325097 -0.0569636 0.998376 -0.0306162
  2265. 0 0 0 1
  2266. shift: 0.163882
  2267. -----------------------------------------------------------------
  2268. (Processed/Input): (119 / 119)
  2269. -----------------------------------------------------------------
  2270. Sequence number: 5049
  2271. Number of scan points: 7283 points.
  2272. Number of filtered scan points: 1267 points.
  2273. transformed_scan_ptr: 7283 points.
  2274. map: 6294 points.
  2275. NDT has converged: 1
  2276. Fitness score: 25.6219
  2277. Number of iteration: 3
  2278. (x,y,z,roll,pitch,yaw):
  2279. (-0.00686913, 0.135246, -0.0330518, -0.0551299, -0.00292746, 0.000314221)
  2280. Transformation Matrix:
  2281. 0.999996 -0.000152437 -0.00294032 -0.00686913
  2282. 0.00031422 0.998481 0.055101 0.135246
  2283. 0.00292746 -0.0551017 0.998476 -0.0330518
  2284. 0 0 0 1
  2285. shift: 0.135421
  2286. -----------------------------------------------------------------
  2287. (Processed/Input): (120 / 120)
  2288. -----------------------------------------------------------------
  2289. Sequence number: 5050
  2290. Number of scan points: 7326 points.
  2291. Number of filtered scan points: 1278 points.
  2292. transformed_scan_ptr: 7326 points.
  2293. map: 6294 points.
  2294. NDT has converged: 1
  2295. Fitness score: 25.9809
  2296. Number of iteration: 3
  2297. (x,y,z,roll,pitch,yaw):
  2298. (0.000383658, 0.152533, -0.0329768, -0.0517321, -0.00437964, -0.00134324)
  2299. Transformation Matrix:
  2300. 0.99999 0.00156791 -0.00430431 0.000383658
  2301. -0.00134323 0.998661 0.0517149 0.152533
  2302. 0.00437963 -0.0517085 0.998653 -0.0329768
  2303. 0 0 0 1
  2304. shift: 0.152534
  2305. -----------------------------------------------------------------
  2306. (Processed/Input): (121 / 121)
  2307. -----------------------------------------------------------------
  2308. Sequence number: 5051
  2309. Number of scan points: 7305 points.
  2310. Number of filtered scan points: 1297 points.
  2311. transformed_scan_ptr: 7305 points.
  2312. map: 6294 points.
  2313. NDT has converged: 1
  2314. Fitness score: 26.0025
  2315. Number of iteration: 2
  2316. (x,y,z,roll,pitch,yaw):
  2317. (0.0191319, 0.132873, -0.0265598, -0.046045, -0.00410037, -0.00257072)
  2318. Transformation Matrix:
  2319. 0.999988 0.00275672 -0.00397768 0.0191319
  2320. -0.00257069 0.998936 0.0460391 0.132873
  2321. 0.00410036 -0.0460283 0.998932 -0.0265598
  2322. 0 0 0 1
  2323. shift: 0.134244
  2324. -----------------------------------------------------------------
  2325. (Processed/Input): (122 / 122)
  2326. -----------------------------------------------------------------
  2327. Sequence number: 5052
  2328. Number of scan points: 7276 points.
  2329. Number of filtered scan points: 1287 points.
  2330. transformed_scan_ptr: 7276 points.
  2331. map: 6294 points.
  2332. NDT has converged: 1
  2333. Fitness score: 26.5321
  2334. Number of iteration: 2
  2335. (x,y,z,roll,pitch,yaw):
  2336. (0.0189404, 0.0766622, -0.0278394, -0.0383174, -0.00234698, -3.78246e-05)
  2337. Transformation Matrix:
  2338. 0.999997 0.000127704 -0.0023438 0.0189404
  2339. -3.78245e-05 0.999266 0.0383081 0.0766622
  2340. 0.00234698 -0.0383079 0.999263 -0.0278394
  2341. 0 0 0 1
  2342. shift: 0.0789673
  2343. -----------------------------------------------------------------
  2344. (Processed/Input): (123 / 123)
  2345. -----------------------------------------------------------------
  2346. Sequence number: 5053
  2347. Number of scan points: 7337 points.
  2348. Number of filtered scan points: 1310 points.
  2349. transformed_scan_ptr: 7337 points.
  2350. map: 6294 points.
  2351. NDT has converged: 1
  2352. Fitness score: 26.8907
  2353. Number of iteration: 2
  2354. (x,y,z,roll,pitch,yaw):
  2355. (0.0257338, 0.066903, -0.0292084, -0.0314757, 0.000193211, -0.00381369)
  2356. Transformation Matrix:
  2357. 0.999993 0.00380571 0.00031313 0.0257338
  2358. -0.00381368 0.999497 0.0314695 0.066903
  2359. -0.000193211 -0.0314705 0.999505 -0.0292084
  2360. 0 0 0 1
  2361. shift: 0.0716815
  2362. -----------------------------------------------------------------
  2363. (Processed/Input): (124 / 124)
  2364. -----------------------------------------------------------------
  2365. Sequence number: 5054
  2366. Number of scan points: 7194 points.
  2367. Number of filtered scan points: 1292 points.
  2368. transformed_scan_ptr: 7194 points.
  2369. map: 6294 points.
  2370. NDT has converged: 1
  2371. Fitness score: 27.2644
  2372. Number of iteration: 2
  2373. (x,y,z,roll,pitch,yaw):
  2374. (0.00784761, 0.0424817, -0.0276488, -0.0236135, 0.00116694, -0.000136461)
  2375. Transformation Matrix:
  2376. 0.999999 0.000108869 0.00116983 0.00784761
  2377. -0.000136461 0.999721 0.0236112 0.0424817
  2378. -0.00116694 -0.0236113 0.999721 -0.0276488
  2379. 0 0 0 1
  2380. shift: 0.0432004
  2381. -----------------------------------------------------------------
  2382. (Processed/Input): (125 / 125)
  2383. -----------------------------------------------------------------
  2384. Sequence number: 5055
  2385. Number of scan points: 7231 points.
  2386. Number of filtered scan points: 1283 points.
  2387. transformed_scan_ptr: 7231 points.
  2388. map: 6294 points.
  2389. NDT has converged: 1
  2390. Fitness score: 26.9718
  2391. Number of iteration: 3
  2392. (x,y,z,roll,pitch,yaw):
  2393. (0.0346694, 0.00742362, -0.0309244, -0.011013, 0.00109989, -0.00155081)
  2394. Transformation Matrix:
  2395. 0.999998 0.0015386 0.00111691 0.0346694
  2396. -0.00155081 0.999938 0.011011 0.00742362
  2397. -0.00109989 -0.0110128 0.999939 -0.0309244
  2398. 0 0 0 1
  2399. shift: 0.0354553
  2400. -----------------------------------------------------------------
  2401. (Processed/Input): (126 / 126)
  2402. -----------------------------------------------------------------
  2403. Sequence number: 5056
  2404. Number of scan points: 7129 points.
  2405. Number of filtered scan points: 1295 points.
  2406. transformed_scan_ptr: 7129 points.
  2407. map: 6294 points.
  2408. NDT has converged: 1
  2409. Fitness score: 27.0703
  2410. Number of iteration: 2
  2411. (x,y,z,roll,pitch,yaw):
  2412. (0.0222326, -0.0258289, -0.0332112, -0.00300699, 0.000460467, -0.0058936)
  2413. Transformation Matrix:
  2414. 0.999983 0.00589215 0.000478182 0.0222326
  2415. -0.00589357 0.999978 0.00300422 -0.0258289
  2416. -0.000460467 -0.00300699 0.999995 -0.0332112
  2417. 0 0 0 1
  2418. shift: 0.0340796
  2419. -----------------------------------------------------------------
  2420. (Processed/Input): (127 / 127)
  2421. -----------------------------------------------------------------
  2422. Sequence number: 5057
  2423. Number of scan points: 6990 points.
  2424. Number of filtered scan points: 1295 points.
  2425. transformed_scan_ptr: 6990 points.
  2426. map: 6294 points.
  2427. NDT has converged: 1
  2428. Fitness score: 26.8237
  2429. Number of iteration: 2
  2430. (x,y,z,roll,pitch,yaw):
  2431. (0.00626604, -0.0314568, -0.0365902, 0.00321551, -0.00024553, -0.00693753)
  2432. Transformation Matrix:
  2433. 0.999976 0.00693664 -0.000267833 0.00626604
  2434. -0.00693747 0.999971 -0.00321373 -0.0314568
  2435. 0.00024553 0.00321551 0.999995 -0.0365902
  2436. 0 0 0 1
  2437. shift: 0.0320748
  2438. -----------------------------------------------------------------
  2439. (Processed/Input): (128 / 128)
  2440. -----------------------------------------------------------------
  2441. Sequence number: 5058
  2442. Number of scan points: 7155 points.
  2443. Number of filtered scan points: 1312 points.
  2444. transformed_scan_ptr: 7155 points.
  2445. map: 6294 points.
  2446. NDT has converged: 1
  2447. Fitness score: 26.8602
  2448. Number of iteration: 3
  2449. (x,y,z,roll,pitch,yaw):
  2450. (0.0447957, -0.0446781, -0.0414114, 0.00909352, -0.00032547, -0.000509477)
  2451. Transformation Matrix:
  2452. 1 0.000506495 -0.000330091 0.0447957
  2453. -0.000509477 0.999959 -0.00909323 -0.0446781
  2454. 0.00032547 0.0090934 0.999959 -0.0414114
  2455. 0 0 0 1
  2456. shift: 0.0632675
  2457. -----------------------------------------------------------------
  2458. (Processed/Input): (129 / 129)
  2459. -----------------------------------------------------------------
  2460. Sequence number: 5059
  2461. Number of scan points: 7124 points.
  2462. Number of filtered scan points: 1301 points.
  2463. transformed_scan_ptr: 7124 points.
  2464. map: 6294 points.
  2465. NDT has converged: 1
  2466. Fitness score: 27.0566
  2467. Number of iteration: 2
  2468. (x,y,z,roll,pitch,yaw):
  2469. (0.0546452, -0.0402249, -0.0402044, 0.0151256, -0.00177421, -0.000574136)
  2470. Transformation Matrix:
  2471. 0.999998 0.000547237 -0.00178269 0.0546452
  2472. -0.000574135 0.999885 -0.015124 -0.0402249
  2473. 0.00177421 0.015125 0.999884 -0.0402044
  2474. 0 0 0 1
  2475. shift: 0.0678538
  2476. -----------------------------------------------------------------
  2477. (Processed/Input): (130 / 130)
  2478. -----------------------------------------------------------------
  2479. Sequence number: 5060
  2480. Number of scan points: 7137 points.
  2481. Number of filtered scan points: 1295 points.
  2482. transformed_scan_ptr: 7137 points.
  2483. map: 6294 points.
  2484. NDT has converged: 1
  2485. Fitness score: 27.0773
  2486. Number of iteration: 3
  2487. (x,y,z,roll,pitch,yaw):
  2488. (0.00857523, -0.0891031, -0.0386838, 0.0243924, -0.00430001, -0.0029264)
  2489. Transformation Matrix:
  2490. 0.999986 0.00282065 -0.00437007 0.00857523
  2491. -0.00292637 0.999699 -0.0243773 -0.0891031
  2492. 0.00429999 0.0243897 0.999693 -0.0386838
  2493. 0 0 0 1
  2494. shift: 0.0895148
  2495. -----------------------------------------------------------------
  2496. (Processed/Input): (131 / 131)
  2497. -----------------------------------------------------------------
  2498. Sequence number: 5061
  2499. Number of scan points: 7305 points.
  2500. Number of filtered scan points: 1343 points.
  2501. transformed_scan_ptr: 7305 points.
  2502. map: 6294 points.
  2503. NDT has converged: 1
  2504. Fitness score: 27.2538
  2505. Number of iteration: 3
  2506. (x,y,z,roll,pitch,yaw):
  2507. (0.0303551, -0.101639, -0.0366099, 0.0342179, -0.0038531, -0.00139076)
  2508. Transformation Matrix:
  2509. 0.999992 0.00125813 -0.00389841 0.0303551
  2510. -0.00139075 0.999414 -0.0342058 -0.101639
  2511. 0.00385309 0.034211 0.999407 -0.0366099
  2512. 0 0 0 1
  2513. shift: 0.106075
  2514. -----------------------------------------------------------------
  2515. (Processed/Input): (132 / 132)
  2516. -----------------------------------------------------------------
  2517. Sequence number: 5062
  2518. Number of scan points: 7421 points.
  2519. Number of filtered scan points: 1365 points.
  2520. transformed_scan_ptr: 7421 points.
  2521. map: 6294 points.
  2522. NDT has converged: 1
  2523. Fitness score: 27.7281
  2524. Number of iteration: 2
  2525. (x,y,z,roll,pitch,yaw):
  2526. (0.0615892, -0.138856, -0.0291789, 0.0438394, -0.00153285, -1.47598e-05)
  2527. Transformation Matrix:
  2528. 0.999999 -5.24316e-05 -0.00153202 0.0615892
  2529. -1.47598e-05 0.999039 -0.0438253 -0.138856
  2530. 0.00153285 0.0438253 0.999038 -0.0291789
  2531. 0 0 0 1
  2532. shift: 0.151902
  2533. -----------------------------------------------------------------
  2534. (Processed/Input): (133 / 133)
  2535. -----------------------------------------------------------------
  2536. Sequence number: 5063
  2537. Number of scan points: 7212 points.
  2538. Number of filtered scan points: 1359 points.
  2539. transformed_scan_ptr: 7212 points.
  2540. map: 6294 points.
  2541. NDT has converged: 1
  2542. Fitness score: 27.3472
  2543. Number of iteration: 3
  2544. (x,y,z,roll,pitch,yaw):
  2545. (0.015472, -0.118521, -0.0240647, 0.0545069, 0.000887762, -0.000781599)
  2546. Transformation Matrix:
  2547. 0.999999 0.000828803 0.000843862 0.015472
  2548. -0.000781599 0.998515 -0.0544806 -0.118521
  2549. -0.000887762 0.0544799 0.998514 -0.0240647
  2550. 0 0 0 1
  2551. shift: 0.119526
  2552. -----------------------------------------------------------------
  2553. (Processed/Input): (134 / 134)
  2554. -----------------------------------------------------------------
  2555. Sequence number: 5064
  2556. Number of scan points: 7206 points.
  2557. Number of filtered scan points: 1347 points.
  2558. transformed_scan_ptr: 7206 points.
  2559. map: 6294 points.
  2560. NDT has converged: 1
  2561. Fitness score: 26.5796
  2562. Number of iteration: 3
  2563. (x,y,z,roll,pitch,yaw):
  2564. (0.0318306, -0.176032, -0.0202559, 0.0664452, 0.00377339, 0.000883863)
  2565. Transformation Matrix:
  2566. 0.999992 -0.000631373 0.00382374 0.0318306
  2567. 0.000883857 0.997793 -0.066393 -0.176032
  2568. -0.00377338 0.0663959 0.997786 -0.0202559
  2569. 0 0 0 1
  2570. shift: 0.178887
  2571. -----------------------------------------------------------------
  2572. (Processed/Input): (135 / 135)
  2573. -----------------------------------------------------------------
  2574. Sequence number: 5065
  2575. Number of scan points: 7278 points.
  2576. Number of filtered scan points: 1359 points.
  2577. transformed_scan_ptr: 7278 points.
  2578. map: 6294 points.
  2579. NDT has converged: 1
  2580. Fitness score: 26.2346
  2581. Number of iteration: 2
  2582. (x,y,z,roll,pitch,yaw):
  2583. (0.0617918, -0.226578, -0.023551, 0.0746609, 0.00429005, 0.00187488)
  2584. Transformation Matrix:
  2585. 0.999989 -0.00154965 0.00441793 0.0617918
  2586. 0.00187486 0.997213 -0.0745834 -0.226578
  2587. -0.00429004 0.0745909 0.997205 -0.023551
  2588. 0 0 0 1
  2589. shift: 0.234852
  2590. -----------------------------------------------------------------
  2591. (Processed/Input): (136 / 136)
  2592. -----------------------------------------------------------------
  2593. Sequence number: 5066
  2594. Number of scan points: 7237 points.
  2595. Number of filtered scan points: 1345 points.
  2596. transformed_scan_ptr: 7237 points.
  2597. map: 6294 points.
  2598. NDT has converged: 1
  2599. Fitness score: 26.9008
  2600. Number of iteration: 3
  2601. (x,y,z,roll,pitch,yaw):
  2602. (0.0567735, -0.211758, -0.0257359, 0.0785888, 0.00403826, 0.00375636)
  2603. Transformation Matrix:
  2604. 0.999985 -0.00342772 0.00432066 0.0567735
  2605. 0.00375632 0.996908 -0.0784922 -0.211758
  2606. -0.00403825 0.0785073 0.996905 -0.0257359
  2607. 0 0 0 1
  2608. shift: 0.219236
  2609. -----------------------------------------------------------------
  2610. (Processed/Input): (137 / 137)
  2611. -----------------------------------------------------------------
  2612. Sequence number: 5067
  2613. Number of scan points: 7294 points.
  2614. Number of filtered scan points: 1389 points.
  2615. transformed_scan_ptr: 7294 points.
  2616. map: 6294 points.
  2617. NDT has converged: 1
  2618. Fitness score: 27.1291
  2619. Number of iteration: 2
  2620. (x,y,z,roll,pitch,yaw):
  2621. (0.0469371, -0.174889, -0.0283088, 0.0787356, 0.00347016, 0.000478587)
  2622. Transformation Matrix:
  2623. 0.999994 -0.000204163 0.00349705 0.0469371
  2624. 0.000478584 0.996902 -0.0786526 -0.174889
  2625. -0.00347015 0.0786538 0.996896 -0.0283088
  2626. 0 0 0 1
  2627. shift: 0.181078
  2628. -----------------------------------------------------------------
  2629. (Processed/Input): (138 / 138)
  2630. -----------------------------------------------------------------
  2631. Sequence number: 5068
  2632. Number of scan points: 7224 points.
  2633. Number of filtered scan points: 1359 points.
  2634. transformed_scan_ptr: 7224 points.
  2635. map: 6294 points.
  2636. NDT has converged: 1
  2637. Fitness score: 26.2572
  2638. Number of iteration: 3
  2639. (x,y,z,roll,pitch,yaw):
  2640. (0.0311445, -0.190609, -0.0291862, 0.0736098, 0.00110611, -0.000298279)
  2641. Transformation Matrix:
  2642. 0.999999 0.000378819 0.00108118 0.0311445
  2643. -0.000298278 0.997292 -0.0735437 -0.190609
  2644. -0.00110611 0.0735433 0.997291 -0.0291862
  2645. 0 0 0 1
  2646. shift: 0.193137
  2647. -----------------------------------------------------------------
  2648. (Processed/Input): (139 / 139)
  2649. -----------------------------------------------------------------
  2650. Sequence number: 5069
  2651. Number of scan points: 7254 points.
  2652. Number of filtered scan points: 1413 points.
  2653. transformed_scan_ptr: 7254 points.
  2654. map: 6294 points.
  2655. NDT has converged: 1
  2656. Fitness score: 26.702
  2657. Number of iteration: 3
  2658. (x,y,z,roll,pitch,yaw):
  2659. (-0.0127686, -0.235002, -0.0313169, 0.0661448, -0.00138082, -0.00124018)
  2660. Transformation Matrix:
  2661. 0.999998 0.0011462 -0.00145977 -0.0127686
  2662. -0.00124017 0.997813 -0.0660948 -0.235002
  2663. 0.00138082 0.0660965 0.997812 -0.0313169
  2664. 0 0 0 1
  2665. shift: 0.235349
  2666. -----------------------------------------------------------------
  2667. (Processed/Input): (140 / 140)
  2668. -----------------------------------------------------------------
  2669. Sequence number: 5070
  2670. Number of scan points: 7421 points.
  2671. Number of filtered scan points: 1381 points.
  2672. transformed_scan_ptr: 7421 points.
  2673. map: 6294 points.
  2674. NDT has converged: 1
  2675. Fitness score: 27.0304
  2676. Number of iteration: 4
  2677. (x,y,z,roll,pitch,yaw):
  2678. (0.00225626, -0.131751, -0.0329548, 0.0540285, -0.00282732, -0.00519893)
  2679. Transformation Matrix:
  2680. 0.999982 0.00503864 -0.0031039 0.00225626
  2681. -0.00519888 0.998528 -0.0539868 -0.131751
  2682. 0.00282732 0.054002 0.998537 -0.0329548
  2683. 0 0 0 1
  2684. shift: 0.13177
  2685. -----------------------------------------------------------------
  2686. (Processed/Input): (141 / 141)
  2687. -----------------------------------------------------------------
  2688. Sequence number: 5071
  2689. Number of scan points: 7291 points.
  2690. Number of filtered scan points: 1351 points.
  2691. transformed_scan_ptr: 7291 points.
  2692. map: 6294 points.
  2693. NDT has converged: 1
  2694. Fitness score: 26.8368
  2695. Number of iteration: 3
  2696. (x,y,z,roll,pitch,yaw):
  2697. (0.0432318, -0.135728, -0.0303857, 0.0411924, -0.00283609, 3.82458e-05)
  2698. Transformation Matrix:
  2699. 0.999996 -0.000155002 -0.0028321 0.0432318
  2700. 3.82456e-05 0.999152 -0.0411808 -0.135728
  2701. 0.00283608 0.0411805 0.999148 -0.0303857
  2702. 0 0 0 1
  2703. shift: 0.142447
  2704. -----------------------------------------------------------------
  2705. (Processed/Input): (142 / 142)
  2706. -----------------------------------------------------------------
  2707. Sequence number: 5072
  2708. Number of scan points: 7382 points.
  2709. Number of filtered scan points: 1350 points.
  2710. transformed_scan_ptr: 7382 points.
  2711. map: 6294 points.
  2712. NDT has converged: 1
  2713. Fitness score: 26.8915
  2714. Number of iteration: 3
  2715. (x,y,z,roll,pitch,yaw):
  2716. (-0.00129579, -0.0992464, -0.0328765, 0.026123, -0.00112726, -0.00575553)
  2717. Transformation Matrix:
  2718. 0.999983 0.00572409 -0.00127721 -0.00129579
  2719. -0.0057555 0.999642 -0.0261131 -0.0992464
  2720. 0.00112726 0.02612 0.999658 -0.0328765
  2721. 0 0 0 1
  2722. shift: 0.0992548
  2723. -----------------------------------------------------------------
  2724. (Processed/Input): (143 / 143)
  2725. -----------------------------------------------------------------
  2726. Sequence number: 5073
  2727. Number of scan points: 7494 points.
  2728. Number of filtered scan points: 1369 points.
  2729. transformed_scan_ptr: 7494 points.
  2730. map: 6294 points.
  2731. NDT has converged: 1
  2732. Fitness score: 27.0969
  2733. Number of iteration: 3
  2734. (x,y,z,roll,pitch,yaw):
  2735. (0.0218422, -0.0525822, -0.0353581, 0.0170763, 0.000668426, 0.00135214)
  2736. Transformation Matrix:
  2737. 0.999999 -0.00134053 0.000691416 0.0218422
  2738. 0.00135214 0.999853 -0.0170746 -0.0525822
  2739. -0.000668426 0.0170755 0.999854 -0.0353581
  2740. 0 0 0 1
  2741. shift: 0.0569383
  2742. -----------------------------------------------------------------
  2743. (Processed/Input): (144 / 144)
  2744. -----------------------------------------------------------------
  2745. Sequence number: 5074
  2746. Number of scan points: 7386 points.
  2747. Number of filtered scan points: 1358 points.
  2748. transformed_scan_ptr: 7386 points.
  2749. map: 6294 points.
  2750. NDT has converged: 1
  2751. Fitness score: 26.6698
  2752. Number of iteration: 3
  2753. (x,y,z,roll,pitch,yaw):
  2754. (0.00864692, -0.0924676, -0.0405845, 0.0106825, 0.00193265, -0.00107217)
  2755. Transformation Matrix:
  2756. 0.999998 0.00109274 0.00192109 0.00864692
  2757. -0.00107217 0.999942 -0.0106844 -0.0924676
  2758. -0.00193265 0.0106823 0.999941 -0.0405845
  2759. 0 0 0 1
  2760. shift: 0.092871
  2761. -----------------------------------------------------------------
  2762. (Processed/Input): (145 / 145)
  2763. -----------------------------------------------------------------
  2764. Sequence number: 5075
  2765. Number of scan points: 7405 points.
  2766. Number of filtered scan points: 1350 points.
  2767. transformed_scan_ptr: 7405 points.
  2768. map: 6294 points.
  2769. NDT has converged: 1
  2770. Fitness score: 26.6014
  2771. Number of iteration: 3
  2772. (x,y,z,roll,pitch,yaw):
  2773. (0.023236, -0.0201137, -0.0445342, 0.00550841, -0.000809378, -0.0022675)
  2774. Transformation Matrix:
  2775. 0.999997 0.00226301 -0.000821851 0.023236
  2776. -0.0022675 0.999982 -0.00550654 -0.0201137
  2777. 0.000809378 0.00550838 0.999985 -0.0445342
  2778. 0 0 0 1
  2779. shift: 0.0307322
  2780. -----------------------------------------------------------------
  2781. (Processed/Input): (146 / 146)
  2782. -----------------------------------------------------------------
  2783. Sequence number: 5076
  2784. Number of scan points: 7666 points.
  2785. Number of filtered scan points: 1336 points.
  2786. transformed_scan_ptr: 7666 points.
  2787. map: 6294 points.
  2788. NDT has converged: 1
  2789. Fitness score: 27.1564
  2790. Number of iteration: 3
  2791. (x,y,z,roll,pitch,yaw):
  2792. (0.0295558, -0.0120297, -0.0451296, 0.00223987, -0.00623609, -0.00350073)
  2793. Transformation Matrix:
  2794. 0.999974 0.00348674 -0.00624384 0.0295558
  2795. -0.00350066 0.999991 -0.00221801 -0.0120297
  2796. 0.00623605 0.00223983 0.999978 -0.0451296
  2797. 0 0 0 1
  2798. shift: 0.0319102
  2799. -----------------------------------------------------------------
  2800. (Processed/Input): (147 / 147)
  2801. -----------------------------------------------------------------
  2802. Sequence number: 5077
  2803. Number of scan points: 7476 points.
  2804. Number of filtered scan points: 1325 points.
  2805. transformed_scan_ptr: 7476 points.
  2806. map: 6294 points.
  2807. NDT has converged: 1
  2808. Fitness score: 27.1329
  2809. Number of iteration: 2
  2810. (x,y,z,roll,pitch,yaw):
  2811. (0.0409506, -0.0149253, -0.0400517, 0.000654922, -0.00955437, -0.00839366)
  2812. Transformation Matrix:
  2813. 0.999919 0.0083873 -0.00955938 0.0409506
  2814. -0.00839317 0.999965 -0.000574699 -0.0149253
  2815. 0.00955422 0.000654892 0.999954 -0.0400517
  2816. 0 0 0 1
  2817. shift: 0.0435857
  2818. -----------------------------------------------------------------
  2819. (Processed/Input): (148 / 148)
  2820. -----------------------------------------------------------------
  2821. Sequence number: 5078
  2822. Number of scan points: 7526 points.
  2823. Number of filtered scan points: 1332 points.
  2824. transformed_scan_ptr: 7526 points.
  2825. map: 6294 points.
  2826. NDT has converged: 1
  2827. Fitness score: 26.6233
  2828. Number of iteration: 4
  2829. (x,y,z,roll,pitch,yaw):
  2830. (-0.00545582, 0.0720231, -0.0366036, -0.00203198, -0.00783109, -0.00621034)
  2831. Transformation Matrix:
  2832. 0.99995 0.0062262 -0.00781823 -0.00545582
  2833. -0.00621011 0.999979 0.00208055 0.0720231
  2834. 0.00783101 -0.00203192 0.999967 -0.0366036
  2835. 0 0 0 1
  2836. shift: 0.0722295
  2837. -----------------------------------------------------------------
  2838. (Processed/Input): (149 / 149)
  2839. -----------------------------------------------------------------
  2840. Sequence number: 5079
  2841. Number of scan points: 7624 points.
  2842. Number of filtered scan points: 1321 points.
  2843. transformed_scan_ptr: 7624 points.
  2844. map: 6294 points.
  2845. NDT has converged: 1
  2846. Fitness score: 26.5332
  2847. Number of iteration: 4
  2848. (x,y,z,roll,pitch,yaw):
  2849. (0.0194779, 0.0348234, -0.0361052, -0.00199737, -0.00344412, -0.00131314)
  2850. Transformation Matrix:
  2851. 0.999993 0.00132004 -0.00344147 0.0194779
  2852. -0.00131313 0.999997 0.00200191 0.0348234
  2853. 0.00344411 -0.00199736 0.999992 -0.0361052
  2854. 0 0 0 1
  2855. shift: 0.0399006
  2856. -----------------------------------------------------------------
  2857. (Processed/Input): (150 / 150)
  2858. -----------------------------------------------------------------
  2859. Sequence number: 5080
  2860. Number of scan points: 7737 points.
  2861. Number of filtered scan points: 1321 points.
  2862. transformed_scan_ptr: 7737 points.
  2863. map: 6294 points.
  2864. NDT has converged: 1
  2865. Fitness score: 26.7247
  2866. Number of iteration: 2
  2867. (x,y,z,roll,pitch,yaw):
  2868. (0.0332124, 0.0157398, -0.0357834, -7.2823e-05, -4.69519e-05, -0.000955666)
  2869. Transformation Matrix:
  2870. 1 0.000955669 -4.68798e-05 0.0332124
  2871. -0.000955665 1 7.28695e-05 0.0157398
  2872. 4.69519e-05 -7.2823e-05 1 -0.0357834
  2873. 0 0 0 1
  2874. shift: 0.0367533
  2875. -----------------------------------------------------------------
  2876. (Processed/Input): (151 / 151)
  2877. -----------------------------------------------------------------
  2878. Sequence number: 5081
  2879. Number of scan points: 7694 points.
  2880. Number of filtered scan points: 1317 points.
  2881. transformed_scan_ptr: 7694 points.
  2882. map: 6294 points.
  2883. NDT has converged: 1
  2884. Fitness score: 26.7753
  2885. Number of iteration: 2
  2886. (x,y,z,roll,pitch,yaw):
  2887. (0.0340195, 0.0175734, -0.0424919, 0.00209918, 0.000513233, -0.00888018)
  2888. Transformation Matrix:
  2889. 0.99996 0.00888112 0.000494564 0.0340195
  2890. -0.00888006 0.999958 -0.00210365 0.0175734
  2891. -0.000513233 0.00209918 0.999998 -0.0424919
  2892. 0 0 0 1
  2893. shift: 0.0382904
  2894. -----------------------------------------------------------------
  2895. (Processed/Input): (152 / 152)
  2896. -----------------------------------------------------------------
  2897. Sequence number: 5082
  2898. Number of scan points: 7838 points.
  2899. Number of filtered scan points: 1311 points.
  2900. transformed_scan_ptr: 7838 points.
  2901. map: 6294 points.
  2902. NDT has converged: 1
  2903. Fitness score: 26.6994
  2904. Number of iteration: 3
  2905. (x,y,z,roll,pitch,yaw):
  2906. (0.0220248, 0.0222601, -0.0473676, 0.00475422, -0.00327161, -0.000343909)
  2907. Transformation Matrix:
  2908. 0.999995 0.00032837 -0.0032732 0.0220248
  2909. -0.000343907 0.999989 -0.00475308 0.0222601
  2910. 0.0032716 0.00475418 0.999983 -0.0473676
  2911. 0 0 0 1
  2912. shift: 0.0313146
  2913. -----------------------------------------------------------------
  2914. (Processed/Input): (153 / 153)
  2915. -----------------------------------------------------------------
  2916. Sequence number: 5083
  2917. Number of scan points: 7899 points.
  2918. Number of filtered scan points: 1280 points.
  2919. transformed_scan_ptr: 7899 points.
  2920. map: 6294 points.
  2921. NDT has converged: 1
  2922. Fitness score: 26.8434
  2923. Number of iteration: 3
  2924. (x,y,z,roll,pitch,yaw):
  2925. (0.038895, -0.0314626, -0.0528095, 0.00584214, -0.00405583, 0.0013786)
  2926. Transformation Matrix:
  2927. 0.999991 -0.0014023 -0.00404768 0.038895
  2928. 0.00137859 0.999982 -0.0058477 -0.0314626
  2929. 0.00405582 0.00584206 0.999975 -0.0528095
  2930. 0 0 0 1
  2931. shift: 0.0500272
  2932. -----------------------------------------------------------------
  2933. (Processed/Input): (154 / 154)
  2934. -----------------------------------------------------------------
  2935. Sequence number: 5084
  2936. Number of scan points: 7791 points.
  2937. Number of filtered scan points: 1262 points.
  2938. transformed_scan_ptr: 7791 points.
  2939. map: 6294 points.
  2940. NDT has converged: 1
  2941. Fitness score: 26.5757
  2942. Number of iteration: 4
  2943. (x,y,z,roll,pitch,yaw):
  2944. (0.0584129, -0.00881163, -0.0529777, 0.0114655, -0.00582401, -0.000817818)
  2945. Transformation Matrix:
  2946. 0.999983 0.000751011 -0.00583297 0.0584129
  2947. -0.000817804 0.999934 -0.0114605 -0.00881163
  2948. 0.00582398 0.0114651 0.999917 -0.0529777
  2949. 0 0 0 1
  2950. shift: 0.0590738
  2951. -----------------------------------------------------------------
  2952. (Processed/Input): (155 / 155)
  2953. -----------------------------------------------------------------
  2954. Sequence number: 5085
  2955. Number of scan points: 7817 points.
  2956. Number of filtered scan points: 1263 points.
  2957. transformed_scan_ptr: 7817 points.
  2958. map: 6294 points.
  2959. NDT has converged: 1
  2960. Fitness score: 26.5611
  2961. Number of iteration: 4
  2962. (x,y,z,roll,pitch,yaw):
  2963. (0.0251445, -0.0469493, -0.0503099, 0.0124893, -0.00513143, -0.0032899)
  2964. Transformation Matrix:
  2965. 0.999981 0.00322557 -0.00517206 0.0251445
  2966. -0.00328985 0.999917 -0.012472 -0.0469493
  2967. 0.00513141 0.0124888 0.999909 -0.0503099
  2968. 0 0 0 1
  2969. shift: 0.0532587
  2970. -----------------------------------------------------------------
  2971. (Processed/Input): (156 / 156)
  2972. -----------------------------------------------------------------
  2973. Sequence number: 5086
  2974. Number of scan points: 7854 points.
  2975. Number of filtered scan points: 1266 points.
  2976. transformed_scan_ptr: 7854 points.
  2977. map: 6294 points.
  2978. NDT has converged: 1
  2979. Fitness score: 26.4141
  2980. Number of iteration: 2
  2981. (x,y,z,roll,pitch,yaw):
  2982. (0.0265637, -0.0782145, -0.0523898, 0.0192876, -0.00429492, -0.00218482)
  2983. Transformation Matrix:
  2984. 0.999988 0.00210157 -0.00433623 0.0265637
  2985. -0.0021848 0.999812 -0.019277 -0.0782145
  2986. 0.0042949 0.0192863 0.999805 -0.0523898
  2987. 0 0 0 1
  2988. shift: 0.0826023
  2989. -----------------------------------------------------------------
  2990. (Processed/Input): (157 / 157)
  2991. -----------------------------------------------------------------
  2992. Sequence number: 5087
  2993. Number of scan points: 7962 points.
  2994. Number of filtered scan points: 1270 points.
  2995. transformed_scan_ptr: 7962 points.
  2996. map: 6294 points.
  2997. NDT has converged: 1
  2998. Fitness score: 26.5552
  2999. Number of iteration: 4
  3000. (x,y,z,roll,pitch,yaw):
  3001. (-0.0368588, -0.0756898, -0.0579301, 0.0277418, -0.00429329, -0.00194628)
  3002. Transformation Matrix:
  3003. 0.999989 0.00182644 -0.0043456 -0.0368588
  3004. -0.00194626 0.999614 -0.0277298 -0.0756898
  3005. 0.00429327 0.0277379 0.999606 -0.0579301
  3006. 0 0 0 1
  3007. shift: 0.0841874
  3008. -----------------------------------------------------------------
  3009. (Processed/Input): (158 / 158)
  3010. -----------------------------------------------------------------
  3011. Sequence number: 5088
  3012. Number of scan points: 7836 points.
  3013. Number of filtered scan points: 1230 points.
  3014. transformed_scan_ptr: 7836 points.
  3015. map: 6294 points.
  3016. NDT has converged: 1
  3017. Fitness score: 26.1038
  3018. Number of iteration: 4
  3019. (x,y,z,roll,pitch,yaw):
  3020. (0.0167895, -0.109621, -0.0563621, 0.0346734, -0.00566695, -0.00304725)
  3021. Transformation Matrix:
  3022. 0.999979 0.00284896 -0.00576913 0.0167895
  3023. -0.00304719 0.999395 -0.034649 -0.109621
  3024. 0.00566692 0.0346659 0.999383 -0.0563621
  3025. 0 0 0 1
  3026. shift: 0.110899
  3027. -----------------------------------------------------------------
  3028. (Processed/Input): (159 / 159)
  3029. -----------------------------------------------------------------
  3030. Sequence number: 5089
  3031. Number of scan points: 8021 points.
  3032. Number of filtered scan points: 1239 points.
  3033. transformed_scan_ptr: 8021 points.
  3034. map: 6294 points.
  3035. NDT has converged: 1
  3036. Fitness score: 26.3871
  3037. Number of iteration: 4
  3038. (x,y,z,roll,pitch,yaw):
  3039. (-0.0128441, -0.0589515, -0.0589724, 0.039945, -0.0052483, -0.00518416)
  3040. Transformation Matrix:
  3041. 0.999973 0.00497042 -0.00545104 -0.0128441
  3042. -0.00518406 0.99919 -0.0399067 -0.0589515
  3043. 0.00524828 0.0399338 0.999189 -0.0589724
  3044. 0 0 0 1
  3045. shift: 0.0603345
  3046. -----------------------------------------------------------------
  3047. (Processed/Input): (160 / 160)
  3048. -----------------------------------------------------------------
  3049. Sequence number: 5090
  3050. Number of scan points: 7996 points.
  3051. Number of filtered scan points: 1290 points.
  3052. transformed_scan_ptr: 7996 points.
  3053. map: 6294 points.
  3054. NDT has converged: 1
  3055. Fitness score: 26.9607
  3056. Number of iteration: 3
  3057. (x,y,z,roll,pitch,yaw):
  3058. (-0.0325749, -0.0858909, -0.0557056, 0.0390083, -0.00611984, -0.00926574)
  3059. Transformation Matrix:
  3060. 0.999938 0.00901991 -0.00647622 -0.0325749
  3061. -0.00926543 0.999199 -0.0389401 -0.0858909
  3062. 0.0061198 0.0389977 0.999221 -0.0557056
  3063. 0 0 0 1
  3064. shift: 0.0918606
  3065. -----------------------------------------------------------------
  3066. (Processed/Input): (161 / 161)
  3067. -----------------------------------------------------------------
  3068. Sequence number: 5091
  3069. Number of scan points: 8109 points.
  3070. Number of filtered scan points: 1246 points.
  3071. transformed_scan_ptr: 8109 points.
  3072. map: 6294 points.
  3073. NDT has converged: 1
  3074. Fitness score: 26.6929
  3075. Number of iteration: 4
  3076. (x,y,z,roll,pitch,yaw):
  3077. (4.67571e-05, -0.0870761, -0.0548296, 0.0351405, -0.00704508, -0.00270456)
  3078. Transformation Matrix:
  3079. 0.999972 0.00245537 -0.00713567 4.67571e-05
  3080. -0.00270449 0.99938 -0.0351141 -0.0870761
  3081. 0.00704502 0.0351324 0.999358 -0.0548296
  3082. 0 0 0 1
  3083. shift: 0.0870761
  3084. -----------------------------------------------------------------
  3085. (Processed/Input): (162 / 162)
  3086. -----------------------------------------------------------------
  3087. Sequence number: 5092
  3088. Number of scan points: 8135 points.
  3089. Number of filtered scan points: 1254 points.
  3090. transformed_scan_ptr: 8135 points.
  3091. map: 6294 points.
  3092. NDT has converged: 1
  3093. Fitness score: 26.6588
  3094. Number of iteration: 3
  3095. (x,y,z,roll,pitch,yaw):
  3096. (0.00826185, -0.109608, -0.0546238, 0.030486, -0.00506037, -0.00332156)
  3097. Transformation Matrix:
  3098. 0.999982 0.00316576 -0.00515922 0.00826185
  3099. -0.00332151 0.99953 -0.0304643 -0.109608
  3100. 0.00506035 0.0304809 0.999523 -0.0546238
  3101. 0 0 0 1
  3102. shift: 0.109919
  3103. -----------------------------------------------------------------
  3104. (Processed/Input): (163 / 163)
  3105. -----------------------------------------------------------------
  3106. Sequence number: 5093
  3107. Number of scan points: 8035 points.
  3108. Number of filtered scan points: 1241 points.
  3109. transformed_scan_ptr: 8035 points.
  3110. map: 6294 points.
  3111. NDT has converged: 1
  3112. Fitness score: 25.7541
  3113. Number of iteration: 3
  3114. (x,y,z,roll,pitch,yaw):
  3115. (6.74163e-05, -0.0495511, -0.0527582, 0.0206139, -0.00332542, -0.00427102)
  3116. Transformation Matrix:
  3117. 0.999985 0.00420155 -0.00341272 6.74163e-05
  3118. -0.00427098 0.999779 -0.020598 -0.0495511
  3119. 0.00332542 0.0206123 0.999782 -0.0527582
  3120. 0 0 0 1
  3121. shift: 0.0495511
  3122. -----------------------------------------------------------------
  3123. (Processed/Input): (164 / 164)
  3124. -----------------------------------------------------------------
  3125. Sequence number: 5094
  3126. Number of scan points: 8172 points.
  3127. Number of filtered scan points: 1229 points.
  3128. transformed_scan_ptr: 8172 points.
  3129. map: 6294 points.
  3130. NDT has converged: 1
  3131. Fitness score: 25.1371
  3132. Number of iteration: 2
  3133. (x,y,z,roll,pitch,yaw):
  3134. (-0.0122484, 0.00450126, -0.0589682, 0.0130838, -0.00213937, -0.00290667)
  3135. Transformation Matrix:
  3136. 0.999994 0.00287842 -0.00217721 -0.0122484
  3137. -0.00290666 0.99991 -0.0130771 0.00450126
  3138. 0.00213936 0.0130834 0.999912 -0.0589682
  3139. 0 0 0 1
  3140. shift: 0.0130493
  3141. -----------------------------------------------------------------
  3142. (Processed/Input): (165 / 165)
  3143. -----------------------------------------------------------------
  3144. Sequence number: 5095
  3145. Number of scan points: 8318 points.
  3146. Number of filtered scan points: 1233 points.
  3147. transformed_scan_ptr: 8318 points.
  3148. map: 6294 points.
  3149. NDT has converged: 1
  3150. Fitness score: 26.2617
  3151. Number of iteration: 3
  3152. (x,y,z,roll,pitch,yaw):
  3153. (-0.00189643, 0.0142544, -0.0542964, 0.00279182, -0.00233623, 0.00331477)
  3154. Transformation Matrix:
  3155. 0.999992 -0.00332129 -0.00232693 -0.00189643
  3156. 0.00331476 0.999991 -0.00279956 0.0142544
  3157. 0.00233623 0.00279181 0.999993 -0.0542964
  3158. 0 0 0 1
  3159. shift: 0.01438
  3160. -----------------------------------------------------------------
  3161. (Processed/Input): (166 / 166)
  3162. -----------------------------------------------------------------
  3163. Sequence number: 5096
  3164. Number of scan points: 8300 points.
  3165. Number of filtered scan points: 1238 points.
  3166. transformed_scan_ptr: 8300 points.
  3167. map: 6294 points.
  3168. NDT has converged: 1
  3169. Fitness score: 26.3683
  3170. Number of iteration: 3
  3171. (x,y,z,roll,pitch,yaw):
  3172. (0.0165698, 0.0504369, -0.058792, -0.00487548, -0.00435324, 0.00776925)
  3173. Transformation Matrix:
  3174. 0.99996 -0.00774787 -0.00439091 0.0165698
  3175. 0.0077691 0.999958 0.0048415 0.0504369
  3176. 0.00435323 -0.00487541 0.999979 -0.058792
  3177. 0 0 0 1
  3178. shift: 0.053089
  3179. -----------------------------------------------------------------
  3180. (Processed/Input): (167 / 167)
  3181. -----------------------------------------------------------------
  3182. Sequence number: 5097
  3183. Number of scan points: 8405 points.
  3184. Number of filtered scan points: 1233 points.
  3185. transformed_scan_ptr: 8405 points.
  3186. map: 6294 points.
  3187. NDT has converged: 1
  3188. Fitness score: 26.0239
  3189. Number of iteration: 4
  3190. (x,y,z,roll,pitch,yaw):
  3191. (0.032465, 0.016221, -0.0641239, -0.00543138, -0.00386544, 0.00254936)
  3192. Transformation Matrix:
  3193. 0.999989 -0.00252832 -0.00387921 0.032465
  3194. 0.00254934 0.999982 0.00542148 0.016221
  3195. 0.00386543 -0.00543132 0.999978 -0.0641239
  3196. 0 0 0 1
  3197. shift: 0.0362918
  3198. -----------------------------------------------------------------
  3199. (Processed/Input): (168 / 168)
  3200. -----------------------------------------------------------------
  3201. Sequence number: 5098
  3202. Number of scan points: 8344 points.
  3203. Number of filtered scan points: 1211 points.
  3204. transformed_scan_ptr: 8344 points.
  3205. map: 6294 points.
  3206. NDT has converged: 1
  3207. Fitness score: 26.1274
  3208. Number of iteration: 2
  3209. (x,y,z,roll,pitch,yaw):
  3210. (-0.00851852, 0.006399, -0.0630711, -0.00772027, -0.00369247, -0.00247701)
  3211. Transformation Matrix:
  3212. 0.99999 0.00250543 -0.00367323 -0.00851852
  3213. -0.00247699 0.999967 0.00772931 0.006399
  3214. 0.00369246 -0.00772014 0.999963 -0.0630711
  3215. 0 0 0 1
  3216. shift: 0.0106542
  3217. -----------------------------------------------------------------
  3218. (Processed/Input): (169 / 169)
  3219. -----------------------------------------------------------------
  3220. Sequence number: 5099
  3221. Number of scan points: 8299 points.
  3222. Number of filtered scan points: 1195 points.
  3223. transformed_scan_ptr: 8299 points.
  3224. map: 6294 points.
  3225. NDT has converged: 1
  3226. Fitness score: 26.0492
  3227. Number of iteration: 2
  3228. (x,y,z,roll,pitch,yaw):
  3229. (-0.0368276, 0.0124083, -0.062151, -0.00953932, -0.00337597, 0.00396136)
  3230. Transformation Matrix:
  3231. 0.999986 -0.00392895 -0.00341359 -0.0368276
  3232. 0.00396132 0.999947 0.00952572 0.0124083
  3233. 0.00337597 -0.00953912 0.999949 -0.062151
  3234. 0 0 0 1
  3235. shift: 0.0388618
  3236. -----------------------------------------------------------------
  3237. (Processed/Input): (170 / 170)
  3238. -----------------------------------------------------------------
  3239. Sequence number: 5100
  3240. Number of scan points: 8344 points.
  3241. Number of filtered scan points: 1195 points.
  3242. transformed_scan_ptr: 8344 points.
  3243. map: 6294 points.
  3244. NDT has converged: 1
  3245. Fitness score: 25.6995
  3246. Number of iteration: 2
  3247. (x,y,z,roll,pitch,yaw):
  3248. (-0.0767269, 0.0111568, -0.0610843, -0.0115625, -0.00364051, -0.00030395)
  3249. Transformation Matrix:
  3250. 0.999993 0.000346039 -0.00363674 -0.0767269
  3251. -0.000303948 0.999933 0.0115634 0.0111568
  3252. 0.0036405 -0.0115622 0.999927 -0.0610843
  3253. 0 0 0 1
  3254. shift: 0.0775338
  3255. -----------------------------------------------------------------
  3256. (Processed/Input): (171 / 171)
  3257. -----------------------------------------------------------------
  3258. Sequence number: 5101
  3259. Number of scan points: 8472 points.
  3260. Number of filtered scan points: 1191 points.
  3261. transformed_scan_ptr: 8472 points.
  3262. map: 6294 points.
  3263. NDT has converged: 1
  3264. Fitness score: 25.2256
  3265. Number of iteration: 3
  3266. (x,y,z,roll,pitch,yaw):
  3267. (-0.0404418, 0.0166068, -0.0745399, -0.00970625, -0.00327622, 0.00459255)
  3268. Transformation Matrix:
  3269. 0.999984 -0.00456052 -0.0033206 -0.0404418
  3270. 0.00459251 0.999942 0.00969095 0.0166068
  3271. 0.00327621 -0.00970604 0.999948 -0.0745399
  3272. 0 0 0 1
  3273. shift: 0.0437187
  3274. -----------------------------------------------------------------
  3275. (Processed/Input): (172 / 172)
  3276. -----------------------------------------------------------------
  3277. Sequence number: 5102
  3278. Number of scan points: 8343 points.
  3279. Number of filtered scan points: 1180 points.
  3280. transformed_scan_ptr: 8343 points.
  3281. map: 6294 points.
  3282. NDT has converged: 1
  3283. Fitness score: 25.4994
  3284. Number of iteration: 2
  3285. (x,y,z,roll,pitch,yaw):
  3286. (-0.0308884, 0.00661241, -0.0698628, -0.0103873, -0.00420248, 0.00273567)
  3287. Transformation Matrix:
  3288. 0.999987 -0.00269187 -0.00423064 -0.0308884
  3289. 0.00273564 0.999942 0.0103756 0.00661241
  3290. 0.00420247 -0.0103871 0.999937 -0.0698628
  3291. 0 0 0 1
  3292. shift: 0.0315883
  3293. -----------------------------------------------------------------
  3294. (Processed/Input): (173 / 173)
  3295. -----------------------------------------------------------------
  3296. Sequence number: 5103
  3297. Number of scan points: 8298 points.
  3298. Number of filtered scan points: 1194 points.
  3299. transformed_scan_ptr: 8298 points.
  3300. map: 6294 points.
  3301. NDT has converged: 1
  3302. Fitness score: 25.3658
  3303. Number of iteration: 3
  3304. (x,y,z,roll,pitch,yaw):
  3305. (-0.048113, 0.0303335, -0.0699999, -0.0102267, -0.00374872, 0.00665562)
  3306. Transformation Matrix:
  3307. 0.999971 -0.00661688 -0.00381652 -0.048113
  3308. 0.00665553 0.999926 0.0102014 0.0303335
  3309. 0.00374872 -0.0102265 0.999941 -0.0699999
  3310. 0 0 0 1
  3311. shift: 0.0568769
  3312. -----------------------------------------------------------------
  3313. (Processed/Input): (174 / 174)
  3314. -----------------------------------------------------------------
  3315. Sequence number: 5104
  3316. Number of scan points: 8308 points.
  3317. Number of filtered scan points: 1164 points.
  3318. transformed_scan_ptr: 8308 points.
  3319. map: 6294 points.
  3320. NDT has converged: 1
  3321. Fitness score: 24.9544
  3322. Number of iteration: 4
  3323. (x,y,z,roll,pitch,yaw):
  3324. (-0.00459517, 0.0382371, -0.0726884, -0.0100576, -0.00395448, 0.00463087)
  3325. Transformation Matrix:
  3326. 0.999981 -0.00459085 -0.0040008 -0.00459517
  3327. 0.00463082 0.999939 0.010039 0.0382371
  3328. 0.00395447 -0.0100573 0.999942 -0.0726884
  3329. 0 0 0 1
  3330. shift: 0.0385122
  3331. -----------------------------------------------------------------
  3332. (Processed/Input): (175 / 175)
  3333. -----------------------------------------------------------------
  3334. Sequence number: 5105
  3335. Number of scan points: 8256 points.
  3336. Number of filtered scan points: 1165 points.
  3337. transformed_scan_ptr: 8256 points.
  3338. map: 6294 points.
  3339. NDT has converged: 1
  3340. Fitness score: 25.5094
  3341. Number of iteration: 3
  3342. (x,y,z,roll,pitch,yaw):
  3343. (0.0123146, -0.00203909, -0.0752026, -0.010309, -0.0047144, -0.00055375)
  3344. Transformation Matrix:
  3345. 0.999989 0.000602332 -0.00470842 0.0123146
  3346. -0.000553744 0.999947 0.0103115 -0.00203909
  3347. 0.00471438 -0.0103087 0.999936 -0.0752026
  3348. 0 0 0 1
  3349. shift: 0.0124823
  3350. -----------------------------------------------------------------
  3351. (Processed/Input): (176 / 176)
  3352. -----------------------------------------------------------------
  3353. Sequence number: 5106
  3354. Number of scan points: 8318 points.
  3355. Number of filtered scan points: 1187 points.
  3356. transformed_scan_ptr: 8318 points.
  3357. map: 6294 points.
  3358. NDT has converged: 1
  3359. Fitness score: 25.8617
  3360. Number of iteration: 4
  3361. (x,y,z,roll,pitch,yaw):
  3362. (0.00442708, 0.0228598, -0.0805056, -0.0092046, -0.0042781, -0.000913275)
  3363. Transformation Matrix:
  3364. 0.99999 0.000952635 -0.0042695 0.00442708
  3365. -0.000913267 0.999957 0.00920838 0.0228598
  3366. 0.00427809 -0.00920439 0.999948 -0.0805056
  3367. 0 0 0 1
  3368. shift: 0.0232846
  3369. -----------------------------------------------------------------
  3370. (Processed/Input): (177 / 177)
  3371. -----------------------------------------------------------------
  3372. Sequence number: 5107
  3373. Number of scan points: 8189 points.
  3374. Number of filtered scan points: 1191 points.
  3375. transformed_scan_ptr: 8189 points.
  3376. map: 6294 points.
  3377. NDT has converged: 1
  3378. Fitness score: 26.0207
  3379. Number of iteration: 3
  3380. (x,y,z,roll,pitch,yaw):
  3381. (-0.00617777, 0.0368639, -0.0834557, -0.00834602, -0.00470616, 0.00782392)
  3382. Transformation Matrix:
  3383. 0.999958 -0.00778429 -0.00477113 -0.00617777
  3384. 0.00782375 0.999935 0.00830885 0.0368639
  3385. 0.00470614 -0.00834583 0.999954 -0.0834557
  3386. 0 0 0 1
  3387. shift: 0.037378
  3388. -----------------------------------------------------------------
  3389. (Processed/Input): (178 / 178)
  3390. -----------------------------------------------------------------
  3391. Sequence number: 5108
  3392. Number of scan points: 8268 points.
  3393. Number of filtered scan points: 1224 points.
  3394. transformed_scan_ptr: 8268 points.
  3395. map: 6294 points.
  3396. NDT has converged: 1
  3397. Fitness score: 27.0485
  3398. Number of iteration: 2
  3399. (x,y,z,roll,pitch,yaw):
  3400. (-0.0455261, 0.05691, -0.0858241, -0.00616855, -0.00502853, 0.0169626)
  3401. Transformation Matrix:
  3402. 0.999843 -0.0169304 -0.00513234 -0.0455261
  3403. 0.0169615 0.999838 0.00608232 0.05691
  3404. 0.00502851 -0.00616844 0.999968 -0.0858241
  3405. 0 0 0 1
  3406. shift: 0.0728792
  3407. -----------------------------------------------------------------
  3408. (Processed/Input): (179 / 179)
  3409. -----------------------------------------------------------------
  3410. Sequence number: 5109
  3411. Number of scan points: 8130 points.
  3412. Number of filtered scan points: 1197 points.
  3413. transformed_scan_ptr: 8130 points.
  3414. map: 6294 points.
  3415. NDT has converged: 1
  3416. Fitness score: 27.1569
  3417. Number of iteration: 4
  3418. (x,y,z,roll,pitch,yaw):
  3419. (-0.0952435, 0.0561557, -0.0692598, -0.00733587, -0.00516021, 0.0163344)
  3420. Transformation Matrix:
  3421. 0.999853 -0.0162953 -0.0052792 -0.0952435
  3422. 0.0163334 0.99984 0.00725053 0.0561557
  3423. 0.00516019 -0.0073357 0.99996 -0.0692598
  3424. 0 0 0 1
  3425. shift: 0.110566
  3426. -----------------------------------------------------------------
  3427. (Processed/Input): (180 / 180)
  3428. -----------------------------------------------------------------
  3429. Sequence number: 5110
  3430. Number of scan points: 8114 points.
  3431. Number of filtered scan points: 1205 points.
  3432. transformed_scan_ptr: 8114 points.
  3433. map: 6294 points.
  3434. NDT has converged: 1
  3435. Fitness score: 27.1783
  3436. Number of iteration: 3
  3437. (x,y,z,roll,pitch,yaw):
  3438. (-0.0938466, 0.0682382, -0.0680877, -0.00754846, -0.00545681, 0.0154915)
  3439. Transformation Matrix:
  3440. 0.999865 -0.0154493 -0.00557293 -0.0938466
  3441. 0.0154907 0.999852 0.00746294 0.0682382
  3442. 0.00545679 -0.00754827 0.999957 -0.0680877
  3443. 0 0 0 1
  3444. shift: 0.116033
  3445. -----------------------------------------------------------------
  3446. (Processed/Input): (181 / 181)
  3447. -----------------------------------------------------------------
  3448. Sequence number: 5111
  3449. Number of scan points: 8171 points.
  3450. Number of filtered scan points: 1232 points.
  3451. transformed_scan_ptr: 8171 points.
  3452. map: 6294 points.
  3453. NDT has converged: 1
  3454. Fitness score: 27.4131
  3455. Number of iteration: 3
  3456. (x,y,z,roll,pitch,yaw):
  3457. (-0.125944, 0.0942106, -0.0715467, -0.0105286, -0.00294111, 0.0163524)
  3458. Transformation Matrix:
  3459. 0.999862 -0.0163198 -0.0031127 -0.125944
  3460. 0.0163516 0.999811 0.0104789 0.0942106
  3461. 0.00294111 -0.0105283 0.99994 -0.0715467
  3462. 0 0 0 1
  3463. shift: 0.157282
  3464. -----------------------------------------------------------------
  3465. (Processed/Input): (182 / 182)
  3466. -----------------------------------------------------------------
  3467. Sequence number: 5112
  3468. Number of scan points: 7999 points.
  3469. Number of filtered scan points: 1241 points.
  3470. transformed_scan_ptr: 7999 points.
  3471. map: 6294 points.
  3472. NDT has converged: 1
  3473. Fitness score: 27.4093
  3474. Number of iteration: 3
  3475. (x,y,z,roll,pitch,yaw):
  3476. (-0.120654, 0.0669056, -0.0669824, -0.0147858, -0.00274987, 0.013144)
  3477. Transformation Matrix:
  3478. 0.99991 -0.0131015 -0.00294366 -0.120654
  3479. 0.0131436 0.999805 0.0147479 0.0669056
  3480. 0.00274987 -0.0147852 0.999887 -0.0669824
  3481. 0 0 0 1
  3482. shift: 0.137963
  3483. -----------------------------------------------------------------
  3484. (Processed/Input): (183 / 183)
  3485. -----------------------------------------------------------------
  3486. Sequence number: 5113
  3487. Number of scan points: 8127 points.
  3488. Number of filtered scan points: 1263 points.
  3489. transformed_scan_ptr: 8127 points.
  3490. map: 6294 points.
  3491. NDT has converged: 1
  3492. Fitness score: 27.4699
  3493. Number of iteration: 4
  3494. (x,y,z,roll,pitch,yaw):
  3495. (-0.0208482, 0.083414, -0.0669238, -0.0175677, -0.00250011, -0.00251025)
  3496. Transformation Matrix:
  3497. 0.999994 0.00255378 -0.00245562 -0.0208482
  3498. -0.00251024 0.999842 0.017573 0.083414
  3499. 0.00250011 -0.0175668 0.999843 -0.0669238
  3500. 0 0 0 1
  3501. shift: 0.0859799
  3502. -----------------------------------------------------------------
  3503. (Processed/Input): (184 / 184)
  3504. -----------------------------------------------------------------
  3505. Sequence number: 5114
  3506. Number of scan points: 7991 points.
  3507. Number of filtered scan points: 1268 points.
  3508. transformed_scan_ptr: 7991 points.
  3509. map: 6294 points.
  3510. NDT has converged: 1
  3511. Fitness score: 27.0953
  3512. Number of iteration: 6
  3513. (x,y,z,roll,pitch,yaw):
  3514. (0.0190588, 0.0756336, -0.0636118, -0.0198815, -0.000626746, -0.00338892)
  3515. Transformation Matrix:
  3516. 0.999994 0.00340071 -0.000559239 0.0190588
  3517. -0.00338892 0.999797 0.0198822 0.0756336
  3518. 0.000626746 -0.0198802 0.999802 -0.0636118
  3519. 0 0 0 1
  3520. shift: 0.0779979
  3521. -----------------------------------------------------------------
  3522. (Processed/Input): (185 / 185)
  3523. -----------------------------------------------------------------
  3524. Sequence number: 5115
  3525. Number of scan points: 7935 points.
  3526. Number of filtered scan points: 1312 points.
  3527. transformed_scan_ptr: 7935 points.
  3528. map: 6294 points.
  3529. NDT has converged: 1
  3530. Fitness score: 27.8241
  3531. Number of iteration: 2
  3532. (x,y,z,roll,pitch,yaw):
  3533. (0.0328881, 0.0792092, -0.0690907, -0.0231114, 0.00101298, -0.0040005)
  3534. Transformation Matrix:
  3535. 0.999991 0.00397601 0.00110516 0.0328881
  3536. -0.00400048 0.999725 0.0231051 0.0792092
  3537. -0.00101298 -0.0231094 0.999732 -0.0690907
  3538. 0 0 0 1
  3539. shift: 0.0857655
  3540. -----------------------------------------------------------------
  3541. (Processed/Input): (186 / 186)
  3542. -----------------------------------------------------------------
  3543. Sequence number: 5116
  3544. Number of scan points: 7874 points.
  3545. Number of filtered scan points: 1274 points.
  3546. transformed_scan_ptr: 7874 points.
  3547. map: 6294 points.
  3548. NDT has converged: 1
  3549. Fitness score: 27.8931
  3550. Number of iteration: 2
  3551. (x,y,z,roll,pitch,yaw):
  3552. (0.0402942, 0.0890737, -0.0624666, -0.0211108, -0.0019727, -0.00234798)
  3553. Transformation Matrix:
  3554. 0.999995 0.0023891 -0.00192269 0.0402942
  3555. -0.00234798 0.999774 0.0211138 0.0890737
  3556. 0.0019727 -0.0211092 0.999775 -0.0624666
  3557. 0 0 0 1
  3558. shift: 0.0977637
  3559. -----------------------------------------------------------------
  3560. (Processed/Input): (187 / 187)
  3561. -----------------------------------------------------------------
  3562. Sequence number: 5117
  3563. Number of scan points: 7767 points.
  3564. Number of filtered scan points: 1252 points.
  3565. transformed_scan_ptr: 7767 points.
  3566. map: 6294 points.
  3567. NDT has converged: 1
  3568. Fitness score: 26.8845
  3569. Number of iteration: 2
  3570. (x,y,z,roll,pitch,yaw):
  3571. (0.0236716, 0.0787701, -0.0702031, -0.0224029, -0.00469595, -0.00131577)
  3572. Transformation Matrix:
  3573. 0.999988 0.00142064 -0.00466527 0.0236716
  3574. -0.00131575 0.999748 0.0224072 0.0787701
  3575. 0.00469593 -0.0224008 0.999738 -0.0702031
  3576. 0 0 0 1
  3577. shift: 0.0822501
  3578. -----------------------------------------------------------------
  3579. (Processed/Input): (188 / 188)
  3580. -----------------------------------------------------------------
  3581. Sequence number: 5118
  3582. Number of scan points: 7914 points.
  3583. Number of filtered scan points: 1304 points.
  3584. transformed_scan_ptr: 7914 points.
  3585. map: 6294 points.
  3586. NDT has converged: 1
  3587. Fitness score: 27.3019
  3588. Number of iteration: 3
  3589. (x,y,z,roll,pitch,yaw):
  3590. (0.0114647, 0.0221298, -0.0527873, -0.0149976, -0.00518848, -0.00811211)
  3591. Transformation Matrix:
  3592. 0.999954 0.00818892 -0.00506605 0.0114647
  3593. -0.00811191 0.999854 0.0150386 0.0221298
  3594. 0.00518846 -0.0149968 0.999874 -0.0527873
  3595. 0 0 0 1
  3596. shift: 0.0249232
  3597. -----------------------------------------------------------------
  3598. (Processed/Input): (189 / 189)
  3599. -----------------------------------------------------------------
  3600. Sequence number: 5119
  3601. Number of scan points: 7783 points.
  3602. Number of filtered scan points: 1299 points.
  3603. transformed_scan_ptr: 7783 points.
  3604. map: 6294 points.
  3605. NDT has converged: 1
  3606. Fitness score: 27.0304
  3607. Number of iteration: 2
  3608. (x,y,z,roll,pitch,yaw):
  3609. (0.0409292, -0.0239848, -0.0501815, -0.0125959, -0.00355789, -0.00756478)
  3610. Transformation Matrix:
  3611. 0.999965 0.00760892 -0.00346222 0.0409292
  3612. -0.00756466 0.999892 0.0126221 -0.0239848
  3613. 0.00355789 -0.0125955 0.999914 -0.0501815
  3614. 0 0 0 1
  3615. shift: 0.0474391
  3616. -----------------------------------------------------------------
  3617. (Processed/Input): (190 / 190)
  3618. -----------------------------------------------------------------
  3619. Sequence number: 5120
  3620. Number of scan points: 7931 points.
  3621. Number of filtered scan points: 1346 points.
  3622. transformed_scan_ptr: 7931 points.
  3623. map: 6294 points.
  3624. NDT has converged: 1
  3625. Fitness score: 28.1241
  3626. Number of iteration: 3
  3627. (x,y,z,roll,pitch,yaw):
  3628. (0.0207143, 0.00370689, -0.0506635, -0.00916813, -0.00334478, -0.00789028)
  3629. Transformation Matrix:
  3630. 0.999963 0.00792053 -0.00327221 0.0207143
  3631. -0.00789016 0.999927 0.00919411 0.00370689
  3632. 0.00334478 -0.00916795 0.999952 -0.0506635
  3633. 0 0 0 1
  3634. shift: 0.0210434
  3635. -----------------------------------------------------------------
  3636. (Processed/Input): (191 / 191)
  3637. -----------------------------------------------------------------
  3638. Sequence number: 5121
  3639. Number of scan points: 7886 points.
  3640. Number of filtered scan points: 1388 points.
  3641. transformed_scan_ptr: 7886 points.
  3642. map: 6294 points.
  3643. NDT has converged: 1
  3644. Fitness score: 28.4724
  3645. Number of iteration: 3
  3646. (x,y,z,roll,pitch,yaw):
  3647. (-0.0274553, 0.0388842, -0.0448617, -0.00216434, -0.00290587, -0.00235491)
  3648. Transformation Matrix:
  3649. 0.999993 0.0023612 -0.00290074 -0.0274553
  3650. -0.0023549 0.999995 0.00217118 0.0388842
  3651. 0.00290586 -0.00216433 0.999993 -0.0448617
  3652. 0 0 0 1
  3653. shift: 0.0476002
  3654. -----------------------------------------------------------------
  3655. (Processed/Input): (192 / 192)
  3656. -----------------------------------------------------------------
  3657. Sequence number: 5122
  3658. Number of scan points: 7942 points.
  3659. Number of filtered scan points: 1420 points.
  3660. transformed_scan_ptr: 7942 points.
  3661. map: 6294 points.
  3662. NDT has converged: 1
  3663. Fitness score: 28.6031
  3664. Number of iteration: 4
  3665. (x,y,z,roll,pitch,yaw):
  3666. (0.0269207, -0.0770206, -0.0414376, 0.00326096, -0.00457072, 0.00118903)
  3667. Transformation Matrix:
  3668. 0.999989 -0.0012039 -0.00456681 0.0269207
  3669. 0.00118901 0.999994 -0.00326638 -0.0770206
  3670. 0.00457071 0.00326092 0.999984 -0.0414376
  3671. 0 0 0 1
  3672. shift: 0.0815898
  3673. -----------------------------------------------------------------
  3674. (Processed/Input): (193 / 193)
  3675. -----------------------------------------------------------------
  3676. Sequence number: 5123
  3677. Number of scan points: 7862 points.
  3678. Number of filtered scan points: 1426 points.
  3679. transformed_scan_ptr: 7862 points.
  3680. map: 6294 points.
  3681. NDT has converged: 1
  3682. Fitness score: 28.2105
  3683. Number of iteration: 5
  3684. (x,y,z,roll,pitch,yaw):
  3685. (0.032652, -0.035986, -0.0283973, 0.00360245, -0.00414802, -0.00180909)
  3686. Transformation Matrix:
  3687. 0.99999 0.00179415 -0.00415448 0.032652
  3688. -0.00180907 0.999992 -0.00359495 -0.035986
  3689. 0.00414801 0.00360241 0.999985 -0.0283973
  3690. 0 0 0 1
  3691. shift: 0.0485916
  3692. -----------------------------------------------------------------
  3693. (Processed/Input): (194 / 194)
  3694. -----------------------------------------------------------------
  3695. Sequence number: 5124
  3696. Number of scan points: 7783 points.
  3697. Number of filtered scan points: 1429 points.
  3698. transformed_scan_ptr: 7783 points.
  3699. map: 6294 points.
  3700. NDT has converged: 1
  3701. Fitness score: 28.0957
  3702. Number of iteration: 3
  3703. (x,y,z,roll,pitch,yaw):
  3704. (0.00707019, -0.0610464, -0.020262, 0.00579402, -0.00444099, -0.00784533)
  3705. Transformation Matrix:
  3706. 0.999959 0.00781938 -0.00448623 0.00707019
  3707. -0.00784517 0.999953 -0.00575896 -0.0610464
  3708. 0.00444097 0.00579393 0.999973 -0.020262
  3709. 0 0 0 1
  3710. shift: 0.0614545
  3711. -----------------------------------------------------------------
  3712. (Processed/Input): (195 / 195)
  3713. -----------------------------------------------------------------
  3714. Sequence number: 5125
  3715. Number of scan points: 7702 points.
  3716. Number of filtered scan points: 1387 points.
  3717. transformed_scan_ptr: 7702 points.
  3718. map: 6294 points.
  3719. NDT has converged: 1
  3720. Fitness score: 28.3316
  3721. Number of iteration: 3
  3722. (x,y,z,roll,pitch,yaw):
  3723. (0.0141061, -0.024128, -0.0171447, 0.00785203, -0.00295611, -0.00467602)
  3724. Transformation Matrix:
  3725. 0.999985 0.00465264 -0.0029927 0.0141061
  3726. -0.00467598 0.999958 -0.00783804 -0.024128
  3727. 0.00295611 0.00785192 0.999965 -0.0171447
  3728. 0 0 0 1
  3729. shift: 0.0279489
  3730. -----------------------------------------------------------------
  3731. (Processed/Input): (196 / 196)
  3732. -----------------------------------------------------------------
  3733. Sequence number: 5126
  3734. Number of scan points: 7629 points.
  3735. Number of filtered scan points: 1376 points.
  3736. transformed_scan_ptr: 7629 points.
  3737. map: 6294 points.
  3738. NDT has converged: 1
  3739. Fitness score: 28.1874
  3740. Number of iteration: 2
  3741. (x,y,z,roll,pitch,yaw):
  3742. (0.0283543, -0.036094, -0.0128679, 0.00827775, -0.00160883, -0.00207041)
  3743. Transformation Matrix:
  3744. 0.999997 0.00205702 -0.0016259 0.0283543
  3745. -0.0020704 0.999964 -0.00827431 -0.036094
  3746. 0.00160883 0.00827764 0.999964 -0.0128679
  3747. 0 0 0 1
  3748. shift: 0.0458993
  3749. -----------------------------------------------------------------
  3750. (Processed/Input): (197 / 197)
  3751. -----------------------------------------------------------------
  3752. Sequence number: 5127
  3753. Number of scan points: 7791 points.
  3754. Number of filtered scan points: 1394 points.
  3755. transformed_scan_ptr: 7791 points.
  3756. map: 6294 points.
  3757. NDT has converged: 1
  3758. Fitness score: 28.0499
  3759. Number of iteration: 3
  3760. (x,y,z,roll,pitch,yaw):
  3761. (0.0232144, -0.0116303, -0.018992, 0.0104893, -0.00151966, -0.00138283)
  3762. Transformation Matrix:
  3763. 0.999998 0.00136681 -0.00153407 0.0232144
  3764. -0.00138282 0.999944 -0.010487 -0.0116303
  3765. 0.00151966 0.0104891 0.999944 -0.018992
  3766. 0 0 0 1
  3767. shift: 0.0259649
  3768. -----------------------------------------------------------------
  3769. (Processed/Input): (198 / 198)
  3770. (Processed/Input): (199 / 199)
  3771. -----------------------------------------------------------------
  3772. Sequence number: 5128
  3773. Number of scan points: 7931 points.
  3774. Number of filtered scan points: 1397 points.
  3775. transformed_scan_ptr: 7931 points.
  3776. map: 6294 points.
  3777. NDT has converged: 1
  3778. Fitness score: 28.2619
  3779. Number of iteration: 3
  3780. (x,y,z,roll,pitch,yaw):
  3781. (0.044866, -0.0244868, -0.0224024, 0.016127, 0.000348965, 0.00240797)
  3782. Transformation Matrix:
  3783. 0.999997 -0.00240203 0.000387746 0.044866
  3784. 0.00240797 0.999867 -0.0161254 -0.0244868
  3785. -0.000348965 0.0161263 0.99987 -0.0224024
  3786. 0 0 0 1
  3787. shift: 0.0511132
  3788. -----------------------------------------------------------------
  3789. -----------------------------------------------------------------
  3790. Sequence number: 5129
  3791. Number of scan points: 7781 points.
  3792. Number of filtered scan points: 1405 points.
  3793. transformed_scan_ptr: 7781 points.
  3794. map: 6294 points.
  3795. NDT has converged: 1
  3796. Fitness score: 28.7845
  3797. Number of iteration: 3
  3798. (x,y,z,roll,pitch,yaw):
  3799. (0.0313537, -0.0561562, -0.0230133, 0.0174921, 0.000651425, 0.000644714)
  3800. Transformation Matrix:
  3801. 1 -0.000633219 0.000662604 0.0313537
  3802. 0.000644713 0.999847 -0.0174908 -0.0561562
  3803. -0.000651425 0.0174913 0.999847 -0.0230133
  3804. 0 0 0 1
  3805. shift: 0.0643162
  3806. -----------------------------------------------------------------
  3807. (Processed/Input): (200 / 200)
  3808. -----------------------------------------------------------------
  3809. Sequence number: 5130
  3810. Number of scan points: 7646 points.
  3811. Number of filtered scan points: 1353 points.
  3812. transformed_scan_ptr: 7646 points.
  3813. map: 6294 points.
  3814. NDT has converged: 1
  3815. Fitness score: 28.7759
  3816. Number of iteration: 3
  3817. (x,y,z,roll,pitch,yaw):
  3818. (0.0247291, -0.0457173, -0.0266789, 0.0166835, -0.000600803, -0.000786361)
  3819. Transformation Matrix:
  3820. 1 0.000776228 -0.000613838 0.0247291
  3821. -0.00078636 0.999861 -0.0166823 -0.0457173
  3822. 0.000600803 0.0166828 0.999861 -0.0266789
  3823. 0 0 0 1
  3824. shift: 0.0519769
  3825. -----------------------------------------------------------------
  3826. (Processed/Input): (201 / 201)
  3827. -----------------------------------------------------------------
  3828. Sequence number: 5131
  3829. Number of scan points: 7844 points.
  3830. Number of filtered scan points: 1389 points.
  3831. transformed_scan_ptr: 7844 points.
  3832. map: 6294 points.
  3833. NDT has converged: 1
  3834. Fitness score: 29.1385
  3835. Number of iteration: 3
  3836. (x,y,z,roll,pitch,yaw):
  3837. (0.0401738, -0.0870751, -0.0360069, 0.0189073, 0.000715269, -0.00306394)
  3838. Transformation Matrix:
  3839. 0.999995 0.00307691 0.000657213 0.0401738
  3840. -0.00306393 0.999817 -0.0189082 -0.0870751
  3841. -0.000715269 0.0189061 0.999821 -0.0360069
  3842. 0 0 0 1
  3843. shift: 0.0958958
  3844. -----------------------------------------------------------------
  3845. (Processed/Input): (202 / 202)
  3846. -----------------------------------------------------------------
  3847. Sequence number: 5132
  3848. Number of scan points: 7826 points.
  3849. Number of filtered scan points: 1397 points.
  3850. transformed_scan_ptr: 7826 points.
  3851. map: 6294 points.
  3852. NDT has converged: 1
  3853. Fitness score: 29.4538
  3854. Number of iteration: 3
  3855. (x,y,z,roll,pitch,yaw):
  3856. (-0.0248496, -0.10618, -0.0358529, 0.0148399, 0.000444091, -0.00997606)
  3857. Transformation Matrix:
  3858. 0.99995 0.00998139 0.000296002 -0.0248496
  3859. -0.0099759 0.99984 -0.014843 -0.10618
  3860. -0.000444091 0.0148393 0.99989 -0.0358529
  3861. 0 0 0 1
  3862. shift: 0.109049
  3863. -----------------------------------------------------------------
  3864. (Processed/Input): (203 / 203)
  3865. -----------------------------------------------------------------
  3866. Sequence number: 5133
  3867. Number of scan points: 7744 points.
  3868. Number of filtered scan points: 1346 points.
  3869. transformed_scan_ptr: 7744 points.
  3870. map: 6294 points.
  3871. NDT has converged: 1
  3872. Fitness score: 29.5996
  3873. Number of iteration: 5
  3874. (x,y,z,roll,pitch,yaw):
  3875. (0.00828794, -0.0651988, -0.0337323, 0.0101017, -0.000313915, -0.0033379)
  3876. Transformation Matrix:
  3877. 0.999994 0.00333456 -0.000347608 0.00828794
  3878. -0.0033379 0.999943 -0.0101005 -0.0651988
  3879. 0.000313915 0.0101016 0.999949 -0.0337323
  3880. 0 0 0 1
  3881. shift: 0.0657234
  3882. -----------------------------------------------------------------
  3883. (Processed/Input): (204 / 204)
  3884. -----------------------------------------------------------------
  3885. Sequence number: 5134
  3886. Number of scan points: 7749 points.
  3887. Number of filtered scan points: 1337 points.
  3888. transformed_scan_ptr: 7749 points.
  3889. map: 6294 points.
  3890. NDT has converged: 1
  3891. Fitness score: 29.5313
  3892. Number of iteration: 2
  3893. (x,y,z,roll,pitch,yaw):
  3894. (-0.0132526, -0.0274017, -0.0316558, 0.00566596, -0.00166966, -0.00717293)
  3895. Transformation Matrix:
  3896. 0.999973 0.00716329 -0.00171025 -0.0132526
  3897. -0.00717286 0.999958 -0.00565381 -0.0274017
  3898. 0.00166966 0.00566593 0.999983 -0.0316558
  3899. 0 0 0 1
  3900. shift: 0.0304382
  3901. -----------------------------------------------------------------
  3902. (Processed/Input): (205 / 205)
  3903. -----------------------------------------------------------------
  3904. Sequence number: 5135
  3905. Number of scan points: 7904 points.
  3906. Number of filtered scan points: 1311 points.
  3907. transformed_scan_ptr: 7904 points.
  3908. map: 6294 points.
  3909. NDT has converged: 1
  3910. Fitness score: 29.3926
  3911. Number of iteration: 3
  3912. (x,y,z,roll,pitch,yaw):
  3913. (0.0282655, -0.00498527, -0.0355699, 0.003207, -0.000892827, -0.00250904)
  3914. Transformation Matrix:
  3915. 0.999996 0.00250615 -0.000900882 0.0282655
  3916. -0.00250903 0.999992 -0.00320474 -0.00498527
  3917. 0.000892827 0.00320699 0.999994 -0.0355699
  3918. 0 0 0 1
  3919. shift: 0.0287018
  3920. -----------------------------------------------------------------
  3921. (Processed/Input): (206 / 206)
  3922. -----------------------------------------------------------------
  3923. Sequence number: 5136
  3924. Number of scan points: 7805 points.
  3925. Number of filtered scan points: 1260 points.
  3926. transformed_scan_ptr: 7805 points.
  3927. map: 6294 points.
  3928. NDT has converged: 1
  3929. Fitness score: 29.2774
  3930. Number of iteration: 3
  3931. (x,y,z,roll,pitch,yaw):
  3932. (0.0348977, -0.0343962, -0.0377596, 0.00350343, 0.00156352, -0.00113419)
  3933. Transformation Matrix:
  3934. 0.999998 0.00113965 0.00155955 0.0348977
  3935. -0.00113419 0.999993 -0.00350519 -0.0343962
  3936. -0.00156352 0.00350342 0.999993 -0.0377596
  3937. 0 0 0 1
  3938. shift: 0.0489995
  3939. -----------------------------------------------------------------
  3940. (Processed/Input): (207 / 207)
  3941. -----------------------------------------------------------------
  3942. Sequence number: 5137
  3943. Number of scan points: 7842 points.
  3944. Number of filtered scan points: 1246 points.
  3945. transformed_scan_ptr: 7842 points.
  3946. map: 6294 points.
  3947. NDT has converged: 1
  3948. Fitness score: 28.5381
  3949. Number of iteration: 2
  3950. (x,y,z,roll,pitch,yaw):
  3951. (0.0588554, -0.0618045, -0.05063, 0.0042964, 0.00225607, -0.00557639)
  3952. Transformation Matrix:
  3953. 0.999982 0.00558601 0.00223205 0.0588554
  3954. -0.00557635 0.999975 -0.0043089 -0.0618045
  3955. -0.00225607 0.00429637 0.999988 -0.05063
  3956. 0 0 0 1
  3957. shift: 0.0853449
  3958. -----------------------------------------------------------------
  3959. (Processed/Input): (208 / 208)
  3960. -----------------------------------------------------------------
  3961. Sequence number: 5138
  3962. Number of scan points: 7834 points.
  3963. Number of filtered scan points: 1214 points.
  3964. transformed_scan_ptr: 7834 points.
  3965. map: 6294 points.
  3966. NDT has converged: 1
  3967. Fitness score: 28.6192
  3968. Number of iteration: 2
  3969. (x,y,z,roll,pitch,yaw):
  3970. (0.0646678, -0.0803488, -0.0623183, 0.00665607, -0.00051686, -0.00242843)
  3971. Transformation Matrix:
  3972. 0.999997 0.00242494 -0.000533 0.0646678
  3973. -0.00242843 0.999975 -0.00665474 -0.0803488
  3974. 0.00051686 0.00665602 0.999978 -0.0623183
  3975. 0 0 0 1
  3976. shift: 0.10314
  3977. -----------------------------------------------------------------
  3978. (Processed/Input): (209 / 209)
  3979. -----------------------------------------------------------------
  3980. Sequence number: 5139
  3981. Number of scan points: 7849 points.
  3982. Number of filtered scan points: 1208 points.
  3983. transformed_scan_ptr: 7849 points.
  3984. map: 6294 points.
  3985. NDT has converged: 1
  3986. Fitness score: 29.9341
  3987. Number of iteration: 4
  3988. (x,y,z,roll,pitch,yaw):
  3989. (0.0304127, -0.0167854, -0.0662003, 0.0101534, -0.00299099, -0.00624215)
  3990. Transformation Matrix:
  3991. 0.999976 0.00621143 -0.00305415 0.0304127
  3992. -0.00624209 0.999929 -0.0101344 -0.0167854
  3993. 0.00299099 0.0101532 0.999944 -0.0662003
  3994. 0 0 0 1
  3995. shift: 0.0347373
  3996. -----------------------------------------------------------------
  3997. (Processed/Input): (210 / 210)
  3998. -----------------------------------------------------------------
  3999. Sequence number: 5140
  4000. Number of scan points: 7841 points.
  4001. Number of filtered scan points: 1202 points.
  4002. transformed_scan_ptr: 7841 points.
  4003. map: 6294 points.
  4004. NDT has converged: 1
  4005. Fitness score: 30.4287
  4006. Number of iteration: 4
  4007. (x,y,z,roll,pitch,yaw):
  4008. (0.0571233, -0.0405708, -0.0510233, 0.0111394, -0.00344795, -0.0013111)
  4009. Transformation Matrix:
  4010. 0.999993 0.0012726 -0.00346234 0.0571233
  4011. -0.0013111 0.999937 -0.0111347 -0.0405708
  4012. 0.00344794 0.0111391 0.999932 -0.0510233
  4013. 0 0 0 1
  4014. shift: 0.0700647
  4015. -----------------------------------------------------------------
  4016. (Processed/Input): (211 / 211)
  4017. -----------------------------------------------------------------
  4018. Sequence number: 5141
  4019. Number of scan points: 8008 points.
  4020. Number of filtered scan points: 1229 points.
  4021. transformed_scan_ptr: 8008 points.
  4022. map: 6294 points.
  4023. NDT has converged: 1
  4024. Fitness score: 29.8821
  4025. Number of iteration: 4
  4026. (x,y,z,roll,pitch,yaw):
  4027. (0.00809955, -0.00611001, -0.0463901, 0.011668, -0.00115343, -0.00410013)
  4028. Transformation Matrix:
  4029. 0.999991 0.00408637 -0.00120119 0.00809955
  4030. -0.00410011 0.999924 -0.0116629 -0.00611001
  4031. 0.00115343 0.0116677 0.999931 -0.0463901
  4032. 0 0 0 1
  4033. shift: 0.0101457
  4034. -----------------------------------------------------------------
  4035. (Processed/Input): (212 / 212)
  4036. -----------------------------------------------------------------
  4037. Sequence number: 5142
  4038. Number of scan points: 7983 points.
  4039. Number of filtered scan points: 1212 points.
  4040. transformed_scan_ptr: 7983 points.
  4041. map: 6294 points.
  4042. NDT has converged: 1
  4043. Fitness score: 29.9114
  4044. Number of iteration: 5
  4045. (x,y,z,roll,pitch,yaw):
  4046. (0.0558217, -0.0686033, -0.0477295, 0.0139878, -0.00364264, 0.00221669)
  4047. Transformation Matrix:
  4048. 0.999991 -0.00226743 -0.00361126 0.0558217
  4049. 0.00221668 0.9999 -0.0139954 -0.0686033
  4050. 0.00364263 0.0139873 0.999896 -0.0477295
  4051. 0 0 0 1
  4052. shift: 0.0884447
  4053. -----------------------------------------------------------------
  4054. (Processed/Input): (213 / 213)
  4055. -----------------------------------------------------------------
  4056. Sequence number: 5143
  4057. Number of scan points: 8122 points.
  4058. Number of filtered scan points: 1251 points.
  4059. transformed_scan_ptr: 8122 points.
  4060. map: 6294 points.
  4061. NDT has converged: 1
  4062. Fitness score: 30.6267
  4063. Number of iteration: 2
  4064. (x,y,z,roll,pitch,yaw):
  4065. (0.083328, -0.0607016, -0.0498598, 0.0144917, -0.000665749, 0.00341155)
  4066. Transformation Matrix:
  4067. 0.999994 -0.00342083 -0.000616245 0.083328
  4068. 0.00341154 0.999889 -0.0144933 -0.0607016
  4069. 0.000665749 0.0144912 0.999895 -0.0498598
  4070. 0 0 0 1
  4071. shift: 0.103093
  4072. -----------------------------------------------------------------
  4073. (Processed/Input): (214 / 214)
  4074. -----------------------------------------------------------------
  4075. Sequence number: 5144
  4076. Number of scan points: 7858 points.
  4077. Number of filtered scan points: 1237 points.
  4078. transformed_scan_ptr: 7858 points.
  4079. map: 6294 points.
  4080. NDT has converged: 1
  4081. Fitness score: 30.2948
  4082. Number of iteration: 2
  4083. (x,y,z,roll,pitch,yaw):
  4084. (0.0805397, -0.0747217, -0.046224, 0.014566, -0.00160663, 0.00190457)
  4085. Transformation Matrix:
  4086. 0.999997 -0.00192777 -0.00157872 0.0805397
  4087. 0.00190457 0.999892 -0.0145685 -0.0747217
  4088. 0.00160663 0.0145654 0.999893 -0.046224
  4089. 0 0 0 1
  4090. shift: 0.109863
  4091. -----------------------------------------------------------------
  4092. (Processed/Input): (215 / 215)
  4093. -----------------------------------------------------------------
  4094. Sequence number: 5145
  4095. Number of scan points: 7842 points.
  4096. Number of filtered scan points: 1251 points.
  4097. transformed_scan_ptr: 7842 points.
  4098. map: 6294 points.
  4099. NDT has converged: 1
  4100. Fitness score: 30.105
  4101. Number of iteration: 3
  4102. (x,y,z,roll,pitch,yaw):
  4103. (0.0656631, -0.0402586, -0.0476449, 0.016316, -0.00279502, -0.00356123)
  4104. Transformation Matrix:
  4105. 0.99999 0.00351514 -0.00285274 0.0656631
  4106. -0.00356121 0.999861 -0.0163052 -0.0402586
  4107. 0.00279502 0.0163152 0.999863 -0.0476449
  4108. 0 0 0 1
  4109. shift: 0.0770221
  4110. -----------------------------------------------------------------
  4111. (Processed/Input): (216 / 216)
  4112. -----------------------------------------------------------------
  4113. Sequence number: 5146
  4114. Number of scan points: 7846 points.
  4115. Number of filtered scan points: 1251 points.
  4116. transformed_scan_ptr: 7846 points.
  4117. map: 6294 points.
  4118. NDT has converged: 1
  4119. Fitness score: 29.5763
  4120. Number of iteration: 2
  4121. (x,y,z,roll,pitch,yaw):
  4122. (0.0251926, 0.000359632, -0.0377522, 0.011619, -0.00290901, -0.00401059)
  4123. Transformation Matrix:
  4124. 0.999988 0.00397651 -0.00295538 0.0251926
  4125. -0.00401056 0.999925 -0.011607 0.000359632
  4126. 0.00290901 0.0116187 0.999928 -0.0377522
  4127. 0 0 0 1
  4128. shift: 0.0251952
  4129. -----------------------------------------------------------------
  4130. (Processed/Input): (217 / 217)
  4131. -----------------------------------------------------------------
  4132. Sequence number: 5147
  4133. Number of scan points: 8015 points.
  4134. Number of filtered scan points: 1260 points.
  4135. transformed_scan_ptr: 8015 points.
  4136. map: 6294 points.
  4137. NDT has converged: 1
  4138. Fitness score: 29.4528
  4139. Number of iteration: 4
  4140. (x,y,z,roll,pitch,yaw):
  4141. (-0.00108288, 0.00604012, -0.0359345, 0.00554944, -0.00154418, -0.00895192)
  4142. Transformation Matrix:
  4143. 0.999959 0.00894309 -0.00159378 -0.00108288
  4144. -0.00895179 0.999945 -0.00553536 0.00604012
  4145. 0.00154418 0.0055494 0.999983 -0.0359345
  4146. 0 0 0 1
  4147. shift: 0.00613642
  4148. -----------------------------------------------------------------
  4149. (Processed/Input): (218 / 218)
  4150. -----------------------------------------------------------------
  4151. Sequence number: 5148
  4152. Number of scan points: 7928 points.
  4153. Number of filtered scan points: 1269 points.
  4154. transformed_scan_ptr: 7928 points.
  4155. map: 6294 points.
  4156. NDT has converged: 1
  4157. Fitness score: 29.6114
  4158. Number of iteration: 3
  4159. (x,y,z,roll,pitch,yaw):
  4160. (0.0117181, 0.00622045, -0.0245687, 0.00314985, -0.00198412, -0.0103565)
  4161. Transformation Matrix:
  4162. 0.999944 0.0103501 -0.00201661 0.0117181
  4163. -0.0103563 0.999942 -0.00312913 0.00622045
  4164. 0.00198412 0.00314983 0.999993 -0.0245687
  4165. 0 0 0 1
  4166. shift: 0.0132668
  4167. -----------------------------------------------------------------
  4168. (Processed/Input): (219 / 219)
  4169. -----------------------------------------------------------------
  4170. Sequence number: 5149
  4171. Number of scan points: 8115 points.
  4172. Number of filtered scan points: 1311 points.
  4173. transformed_scan_ptr: 8115 points.
  4174. map: 6294 points.
  4175. NDT has converged: 1
  4176. Fitness score: 29.5265
  4177. Number of iteration: 2
  4178. (x,y,z,roll,pitch,yaw):
  4179. (0.0449237, 0.00883929, -0.0173159, 0.00244705, -0.00125242, -0.00285885)
  4180. Transformation Matrix:
  4181. 0.999995 0.00285577 -0.00125941 0.0449237
  4182. -0.00285884 0.999993 -0.00244346 0.00883929
  4183. 0.00125242 0.00244705 0.999996 -0.0173159
  4184. 0 0 0 1
  4185. shift: 0.045785
  4186. -----------------------------------------------------------------
  4187. (Processed/Input): (220 / 220)
  4188. -----------------------------------------------------------------
  4189. Sequence number: 5150
  4190. Number of scan points: 7910 points.
  4191. Number of filtered scan points: 1327 points.
  4192. transformed_scan_ptr: 7910 points.
  4193. map: 6294 points.
  4194. NDT has converged: 1
  4195. Fitness score: 28.8332
  4196. Number of iteration: 4
  4197. (x,y,z,roll,pitch,yaw):
  4198. (0.00635932, -0.0180055, -0.00853132, 9.30245e-05, -0.0016989, -0.00324778)
  4199. Transformation Matrix:
  4200. 0.999993 0.00324761 -0.00169919 0.00635932
  4201. -0.00324777 0.999995 -8.74948e-05 -0.0180055
  4202. 0.0016989 9.30244e-05 0.999999 -0.00853132
  4203. 0 0 0 1
  4204. shift: 0.0190956
  4205. -----------------------------------------------------------------
  4206. (Processed/Input): (221 / 221)
  4207. -----------------------------------------------------------------
  4208. Sequence number: 5151
  4209. Number of scan points: 8055 points.
  4210. Number of filtered scan points: 1340 points.
  4211. transformed_scan_ptr: 8055 points.
  4212. map: 6294 points.
  4213. NDT has converged: 1
  4214. Fitness score: 28.5421
  4215. Number of iteration: 4
  4216. (x,y,z,roll,pitch,yaw):
  4217. (0.0748613, 0.0455188, -0.00901008, 0.00229704, 0.00110455, -0.00130224)
  4218. Transformation Matrix:
  4219. 0.999999 0.00130478 0.00110155 0.0748613
  4220. -0.00130224 0.999996 -0.00229848 0.0455188
  4221. -0.00110455 0.00229703 0.999997 -0.00901008
  4222. 0 0 0 1
  4223. shift: 0.0876138
  4224. -----------------------------------------------------------------
  4225. (Processed/Input): (222 / 222)
  4226. -----------------------------------------------------------------
  4227. Sequence number: 5152
  4228. Number of scan points: 8075 points.
  4229. Number of filtered scan points: 1363 points.
  4230. transformed_scan_ptr: 8075 points.
  4231. map: 6294 points.
  4232. NDT has converged: 1
  4233. Fitness score: 28.333
  4234. Number of iteration: 4
  4235. (x,y,z,roll,pitch,yaw):
  4236. (0.0348317, 0.0406486, -0.0148413, 0.00591601, 0.00115868, -0.00350142)
  4237. Transformation Matrix:
  4238. 0.999993 0.00350821 0.00113794 0.0348317
  4239. -0.00350141 0.999976 -0.00592 0.0406486
  4240. -0.00115868 0.00591597 0.999982 -0.0148413
  4241. 0 0 0 1
  4242. shift: 0.0535309
  4243. -----------------------------------------------------------------
  4244. (Processed/Input): (223 / 223)
  4245. -----------------------------------------------------------------
  4246. Sequence number: 5153
  4247. Number of scan points: 7840 points.
  4248. Number of filtered scan points: 1338 points.
  4249. transformed_scan_ptr: 7840 points.
  4250. map: 6294 points.
  4251. NDT has converged: 1
  4252. Fitness score: 27.5763
  4253. Number of iteration: 2
  4254. (x,y,z,roll,pitch,yaw):
  4255. (0.00095639, 0.00339218, -0.00904606, 0.00853511, -0.000803071, -0.00325863)
  4256. Transformation Matrix:
  4257. 0.999994 0.00325166 -0.000830835 0.00095639
  4258. -0.00325863 0.999958 -0.00853235 0.00339218
  4259. 0.000803071 0.00853501 0.999963 -0.00904606
  4260. 0 0 0 1
  4261. shift: 0.00352443
  4262. -----------------------------------------------------------------
  4263. (Processed/Input): (224 / 224)
  4264. -----------------------------------------------------------------
  4265. Sequence number: 5154
  4266. Number of scan points: 7807 points.
  4267. Number of filtered scan points: 1327 points.
  4268. transformed_scan_ptr: 7807 points.
  4269. map: 6294 points.
  4270. NDT has converged: 1
  4271. Fitness score: 27.4771
  4272. Number of iteration: 2
  4273. (x,y,z,roll,pitch,yaw):
  4274. (0.00174911, -0.0647229, -0.0106508, 0.0133257, -0.000385657, -0.00638216)
  4275. Transformation Matrix:
  4276. 0.99998 0.00637641 -0.000470648 0.00174911
  4277. -0.00638212 0.999891 -0.0133225 -0.0647229
  4278. 0.000385657 0.0133253 0.999911 -0.0106508
  4279. 0 0 0 1
  4280. shift: 0.0647465
  4281. -----------------------------------------------------------------
  4282. (Processed/Input): (225 / 225)
  4283. -----------------------------------------------------------------
  4284. Sequence number: 5155
  4285. Number of scan points: 7979 points.
  4286. Number of filtered scan points: 1373 points.
  4287. transformed_scan_ptr: 7979 points.
  4288. map: 6294 points.
  4289. NDT has converged: 1
  4290. Fitness score: 27.4853
  4291. Number of iteration: 4
  4292. (x,y,z,roll,pitch,yaw):
  4293. (-0.00308548, -0.0653253, -0.0130443, 0.0131187, 0.000961482, -0.00752167)
  4294. Transformation Matrix:
  4295. 0.999971 0.00753357 0.00086269 -0.00308548
  4296. -0.0075216 0.999886 -0.0131251 -0.0653253
  4297. -0.000961482 0.0131183 0.999913 -0.0130443
  4298. 0 0 0 1
  4299. shift: 0.0653982
  4300. -----------------------------------------------------------------
  4301. (Processed/Input): (226 / 226)
  4302. -----------------------------------------------------------------
  4303. Sequence number: 5156
  4304. Number of scan points: 7680 points.
  4305. Number of filtered scan points: 1360 points.
  4306. transformed_scan_ptr: 7680 points.
  4307. map: 6294 points.
  4308. NDT has converged: 1
  4309. Fitness score: 26.5877
  4310. Number of iteration: 3
  4311. (x,y,z,roll,pitch,yaw):
  4312. (0.00547567, -0.029764, -0.00668601, 0.009938, -0.000574583, -0.00374143)
  4313. Transformation Matrix:
  4314. 0.999993 0.00373552 -0.000611752 0.00547567
  4315. -0.00374142 0.999944 -0.00993561 -0.029764
  4316. 0.000574583 0.00993783 0.99995 -0.00668601
  4317. 0 0 0 1
  4318. shift: 0.0302634
  4319. -----------------------------------------------------------------
  4320. (Processed/Input): (227 / 227)
  4321. -----------------------------------------------------------------
  4322. Sequence number: 5157
  4323. Number of scan points: 7950 points.
  4324. Number of filtered scan points: 1374 points.
  4325. transformed_scan_ptr: 7950 points.
  4326. map: 6294 points.
  4327. NDT has converged: 1
  4328. Fitness score: 26.0135
  4329. Number of iteration: 2
  4330. (x,y,z,roll,pitch,yaw):
  4331. (0.0153684, -0.009521, -0.0115581, 0.00749574, -0.000214423, -0.00385736)
  4332. Transformation Matrix:
  4333. 0.999993 0.00385564 -0.000243306 0.0153684
  4334. -0.00385735 0.999964 -0.00749479 -0.009521
  4335. 0.000214423 0.00749567 0.999972 -0.0115581
  4336. 0 0 0 1
  4337. shift: 0.0180786
  4338. -----------------------------------------------------------------
  4339. (Processed/Input): (228 / 228)
  4340. -----------------------------------------------------------------
  4341. Sequence number: 5158
  4342. Number of scan points: 7735 points.
  4343. Number of filtered scan points: 1398 points.
  4344. transformed_scan_ptr: 7735 points.
  4345. map: 6294 points.
  4346. NDT has converged: 1
  4347. Fitness score: 26.6154
  4348. Number of iteration: 2
  4349. (x,y,z,roll,pitch,yaw):
  4350. (-0.024556, 0.00749326, 0.00372043, 0.00255457, -0.00192917, -0.00288808)
  4351. Transformation Matrix:
  4352. 0.999994 0.00288313 -0.00193654 -0.024556
  4353. -0.00288807 0.999993 -0.00254898 0.00749326
  4354. 0.00192917 0.00255457 0.999995 0.00372043
  4355. 0 0 0 1
  4356. shift: 0.0256739
  4357. -----------------------------------------------------------------
  4358. (Processed/Input): (229 / 229)
  4359. -----------------------------------------------------------------
  4360. Sequence number: 5159
  4361. Number of scan points: 7874 points.
  4362. Number of filtered scan points: 1364 points.
  4363. transformed_scan_ptr: 7874 points.
  4364. map: 6294 points.
  4365. NDT has converged: 1
  4366. Fitness score: 26.3883
  4367. Number of iteration: 3
  4368. (x,y,z,roll,pitch,yaw):
  4369. (0.0227486, -0.0309777, 0.0125413, 0.0009881, -0.00167846, 0.00151925)
  4370. Transformation Matrix:
  4371. 0.999997 -0.0015209 -0.00167696 0.0227486
  4372. 0.00151925 0.999998 -0.000990638 -0.0309777
  4373. 0.00167846 0.000988098 0.999998 0.0125413
  4374. 0 0 0 1
  4375. shift: 0.0384333
  4376. -----------------------------------------------------------------
  4377. (Processed/Input): (230 / 230)
  4378. -----------------------------------------------------------------
  4379. Sequence number: 5160
  4380. Number of scan points: 7873 points.
  4381. Number of filtered scan points: 1344 points.
  4382. transformed_scan_ptr: 7873 points.
  4383. map: 6294 points.
  4384. NDT has converged: 1
  4385. Fitness score: 26.9618
  4386. Number of iteration: 3
  4387. (x,y,z,roll,pitch,yaw):
  4388. (0.043729, -0.0365165, 0.0171647, 0.00167438, 0.00129951, -0.000861826)
  4389. Transformation Matrix:
  4390. 0.999999 0.000864004 0.00129807 0.043729
  4391. -0.000861825 0.999998 -0.0016755 -0.0365165
  4392. -0.00129951 0.00167438 0.999998 0.0171647
  4393. 0 0 0 1
  4394. shift: 0.0569709
  4395. -----------------------------------------------------------------
  4396. (Processed/Input): (231 / 231)
  4397. -----------------------------------------------------------------
  4398. Sequence number: 5161
  4399. Number of scan points: 7810 points.
  4400. Number of filtered scan points: 1361 points.
  4401. transformed_scan_ptr: 7810 points.
  4402. map: 6294 points.
  4403. NDT has converged: 1
  4404. Fitness score: 27.3094
  4405. Number of iteration: 3
  4406. (x,y,z,roll,pitch,yaw):
  4407. (0.0211566, -0.0154897, 0.019619, 0.000419022, 0.00154244, 0.00278933)
  4408. Transformation Matrix:
  4409. 0.999995 -0.00278867 0.0015436 0.0211566
  4410. 0.00278932 0.999996 -0.000414702 -0.0154897
  4411. -0.00154244 0.000419021 0.999999 0.019619
  4412. 0 0 0 1
  4413. shift: 0.0262208
  4414. -----------------------------------------------------------------
  4415. (Processed/Input): (232 / 232)
  4416. -----------------------------------------------------------------
  4417. Sequence number: 5162
  4418. Number of scan points: 7701 points.
  4419. Number of filtered scan points: 1358 points.
  4420. transformed_scan_ptr: 7701 points.
  4421. map: 6294 points.
  4422. NDT has converged: 1
  4423. Fitness score: 27.4106
  4424. Number of iteration: 2
  4425. (x,y,z,roll,pitch,yaw):
  4426. (0.0564978, -0.0212409, 0.0115475, 0.00136295, 0.00151976, -0.00116525)
  4427. Transformation Matrix:
  4428. 0.999998 0.0011673 0.00151818 0.0564978
  4429. -0.00116524 0.999998 -0.0013647 -0.0212409
  4430. -0.00151976 0.00136294 0.999998 0.0115475
  4431. 0 0 0 1
  4432. shift: 0.0603588
  4433. -----------------------------------------------------------------
  4434. (Processed/Input): (233 / 233)
  4435. -----------------------------------------------------------------
  4436. Sequence number: 5163
  4437. Number of scan points: 7637 points.
  4438. Number of filtered scan points: 1358 points.
  4439. transformed_scan_ptr: 7637 points.
  4440. map: 6294 points.
  4441. NDT has converged: 1
  4442. Fitness score: 27.7809
  4443. Number of iteration: 4
  4444. (x,y,z,roll,pitch,yaw):
  4445. (0.0320243, -0.00590918, -0.00277933, 0.00261988, 0.00253872, -0.00172312)
  4446. Transformation Matrix:
  4447. 0.999995 0.00172978 0.00253418 0.0320243
  4448. -0.00172311 0.999995 -0.00262426 -0.00590918
  4449. -0.00253872 0.00261987 0.999993 -0.00277933
  4450. 0 0 0 1
  4451. shift: 0.0325649
  4452. -----------------------------------------------------------------
  4453. (Processed/Input): (234 / 234)
  4454. -----------------------------------------------------------------
  4455. Sequence number: 5164
  4456. Number of scan points: 7780 points.
  4457. Number of filtered scan points: 1336 points.
  4458. transformed_scan_ptr: 7780 points.
  4459. map: 6294 points.
  4460. NDT has converged: 1
  4461. Fitness score: 27.2708
  4462. Number of iteration: 2
  4463. (x,y,z,roll,pitch,yaw):
  4464. (0.020381, -0.00119912, -0.0192271, 0.00343734, 0.00246311, -0.00427226)
  4465. Transformation Matrix:
  4466. 0.999988 0.00428068 0.00244841 0.020381
  4467. -0.00427224 0.999985 -0.00344781 -0.00119912
  4468. -0.00246311 0.00343732 0.999991 -0.0192271
  4469. 0 0 0 1
  4470. shift: 0.0204163
  4471. -----------------------------------------------------------------
  4472. (Processed/Input): (235 / 235)
  4473. -----------------------------------------------------------------
  4474. Sequence number: 5165
  4475. Number of scan points: 7894 points.
  4476. Number of filtered scan points: 1331 points.
  4477. transformed_scan_ptr: 7894 points.
  4478. map: 6294 points.
  4479. NDT has converged: 1
  4480. Fitness score: 27.1698
  4481. Number of iteration: 3
  4482. (x,y,z,roll,pitch,yaw):
  4483. (0.0615665, -0.0238644, -0.0393917, 0.00623765, 0.00269428, 0.00192641)
  4484. Transformation Matrix:
  4485. 0.999995 -0.00190957 0.00270624 0.0615665
  4486. 0.0019264 0.999979 -0.00623241 -0.0238644
  4487. -0.00269428 0.00623759 0.999977 -0.0393917
  4488. 0 0 0 1
  4489. shift: 0.0660299
  4490. -----------------------------------------------------------------
  4491. (Processed/Input): (236 / 236)
  4492. -----------------------------------------------------------------
  4493. Sequence number: 5166
  4494. Number of scan points: 7957 points.
  4495. Number of filtered scan points: 1323 points.
  4496. transformed_scan_ptr: 7957 points.
  4497. map: 6294 points.
  4498. NDT has converged: 1
  4499. Fitness score: 26.6291
  4500. Number of iteration: 2
  4501. (x,y,z,roll,pitch,yaw):
  4502. (0.0887279, -0.0454633, -0.0372775, 0.00406925, 0.00361678, 0.00718346)
  4503. Transformation Matrix:
  4504. 0.999968 -0.00716861 0.0036459 0.0887279
  4505. 0.00718335 0.999966 -0.00404314 -0.0454633
  4506. -0.00361677 0.00406921 0.999985 -0.0372775
  4507. 0 0 0 1
  4508. shift: 0.0996973
  4509. -----------------------------------------------------------------
  4510. (Processed/Input): (237 / 237)
  4511. -----------------------------------------------------------------
  4512. Sequence number: 5167
  4513. Number of scan points: 8043 points.
  4514. Number of filtered scan points: 1300 points.
  4515. transformed_scan_ptr: 8043 points.
  4516. map: 6294 points.
  4517. NDT has converged: 1
  4518. Fitness score: 27.0803
  4519. Number of iteration: 3
  4520. (x,y,z,roll,pitch,yaw):
  4521. (0.0540359, -0.0386743, -0.0552513, 0.00671753, 0.00176724, -0.00186471)
  4522. Transformation Matrix:
  4523. 0.999997 0.00187654 0.00175467 0.0540359
  4524. -0.00186471 0.999976 -0.00672076 -0.0386743
  4525. -0.00176724 0.00671747 0.999976 -0.0552513
  4526. 0 0 0 1
  4527. shift: 0.0664499
  4528. -----------------------------------------------------------------
  4529. (Processed/Input): (238 / 238)
  4530. -----------------------------------------------------------------
  4531. Sequence number: 5168
  4532. Number of scan points: 7907 points.
  4533. Number of filtered scan points: 1293 points.
  4534. transformed_scan_ptr: 7907 points.
  4535. map: 6294 points.
  4536. NDT has converged: 1
  4537. Fitness score: 27.192
  4538. Number of iteration: 3
  4539. (x,y,z,roll,pitch,yaw):
  4540. (-0.0267551, -0.00755634, -0.0469453, 0.00525725, 0.0020856, -0.00267985)
  4541. Transformation Matrix:
  4542. 0.999994 0.00269078 0.00207147 -0.0267551
  4543. -0.00267984 0.999983 -0.0052628 -0.00755634
  4544. -0.0020856 0.00525721 0.999984 -0.0469453
  4545. 0 0 0 1
  4546. shift: 0.0278017
  4547. -----------------------------------------------------------------
  4548. (Processed/Input): (239 / 239)
  4549. -----------------------------------------------------------------
  4550. Sequence number: 5169
  4551. Number of scan points: 7870 points.
  4552. Number of filtered scan points: 1316 points.
  4553. transformed_scan_ptr: 7870 points.
  4554. map: 6294 points.
  4555. NDT has converged: 1
  4556. Fitness score: 27.0129
  4557. Number of iteration: 5
  4558. (x,y,z,roll,pitch,yaw):
  4559. (0.0530727, 0.0087941, -0.0597686, 0.00807794, 0.00321478, 0.00280021)
  4560. Transformation Matrix:
  4561. 0.999991 -0.00277416 0.00323727 0.0530727
  4562. 0.00280019 0.999964 -0.00806882 0.0087941
  4563. -0.00321477 0.00807781 0.999962 -0.0597686
  4564. 0 0 0 1
  4565. shift: 0.0537964
  4566. -----------------------------------------------------------------
  4567. (Processed/Input): (240 / 240)
  4568. -----------------------------------------------------------------
  4569. Sequence number: 5170
  4570. Number of scan points: 7942 points.
  4571. Number of filtered scan points: 1295 points.
  4572. transformed_scan_ptr: 7942 points.
  4573. map: 6294 points.
  4574. NDT has converged: 1
  4575. Fitness score: 26.5965
  4576. Number of iteration: 3
  4577. (x,y,z,roll,pitch,yaw):
  4578. (0.0885222, 0.0163017, -0.059322, 0.00606238, 0.000296522, 0.00956642)
  4579. Transformation Matrix:
  4580. 0.999954 -0.0095643 0.00035452 0.0885222
  4581. 0.00956627 0.999936 -0.00605923 0.0163017
  4582. -0.000296522 0.00606234 0.999982 -0.059322
  4583. 0 0 0 1
  4584. shift: 0.0900107
  4585. -----------------------------------------------------------------
  4586. (Processed/Input): (241 / 241)
  4587. -----------------------------------------------------------------
  4588. Sequence number: 5171
  4589. Number of scan points: 8002 points.
  4590. Number of filtered scan points: 1292 points.
  4591. transformed_scan_ptr: 8002 points.
  4592. map: 6294 points.
  4593. NDT has converged: 1
  4594. Fitness score: 26.4117
  4595. Number of iteration: 5
  4596. (x,y,z,roll,pitch,yaw):
  4597. (-0.0182276, -0.0128681, -0.0620757, 0.00774551, -0.00183992, -0.00086795)
  4598. Transformation Matrix:
  4599. 0.999998 0.00085367 -0.00184658 -0.0182276
  4600. -0.000867949 0.99997 -0.00774383 -0.0128681
  4601. 0.00183992 0.00774542 0.999968 -0.0620757
  4602. 0 0 0 1
  4603. shift: 0.0223122
  4604. -----------------------------------------------------------------
  4605. (Processed/Input): (242 / 242)
  4606. -----------------------------------------------------------------
  4607. Sequence number: 5172
  4608. Number of scan points: 8094 points.
  4609. Number of filtered scan points: 1291 points.
  4610. transformed_scan_ptr: 8094 points.
  4611. map: 6294 points.
  4612. NDT has converged: 1
  4613. Fitness score: 26.6301
  4614. Number of iteration: 5
  4615. (x,y,z,roll,pitch,yaw):
  4616. (-0.0271686, -0.0241566, -0.059718, 0.00833182, -0.00149583, -0.00642011)
  4617. Transformation Matrix:
  4618. 0.999978 0.00640738 -0.00154923 -0.0271686
  4619. -0.00642006 0.999945 -0.00832195 -0.0241566
  4620. 0.00149583 0.00833171 0.999964 -0.059718
  4621. 0 0 0 1
  4622. shift: 0.0363548
  4623. -----------------------------------------------------------------
  4624. (Processed/Input): (243 / 243)
  4625. -----------------------------------------------------------------
  4626. Sequence number: 5173
  4627. Number of scan points: 8136 points.
  4628. Number of filtered scan points: 1279 points.
  4629. transformed_scan_ptr: 8136 points.
  4630. map: 6294 points.
  4631. NDT has converged: 1
  4632. Fitness score: 26.5518
  4633. Number of iteration: 3
  4634. (x,y,z,roll,pitch,yaw):
  4635. (-0.022017, 0.00239305, -0.0615903, 0.00860766, -0.000680069, -0.00489939)
  4636. Transformation Matrix:
  4637. 0.999988 0.00489334 -0.000722232 -0.022017
  4638. -0.00489937 0.999951 -0.00860412 0.00239305
  4639. 0.000680069 0.00860756 0.999963 -0.0615903
  4640. 0 0 0 1
  4641. shift: 0.0221467
  4642. -----------------------------------------------------------------
  4643. (Processed/Input): (244 / 244)
  4644. -----------------------------------------------------------------
  4645. Sequence number: 5174
  4646. Number of scan points: 8104 points.
  4647. Number of filtered scan points: 1255 points.
  4648. transformed_scan_ptr: 8104 points.
  4649. map: 6294 points.
  4650. NDT has converged: 1
  4651. Fitness score: 26.1825
  4652. Number of iteration: 5
  4653. (x,y,z,roll,pitch,yaw):
  4654. (0.0365018, -0.038508, -0.0580871, 0.00798302, 0.000407949, -0.00355593)
  4655. Transformation Matrix:
  4656. 0.999994 0.00355907 0.000379554 0.0365018
  4657. -0.00355593 0.999962 -0.00798433 -0.038508
  4658. -0.000407949 0.00798293 0.999968 -0.0580871
  4659. 0 0 0 1
  4660. shift: 0.0530589
  4661. -----------------------------------------------------------------
  4662. (Processed/Input): (245 / 245)
  4663. -----------------------------------------------------------------
  4664. Sequence number: 5175
  4665. Number of scan points: 8177 points.
  4666. Number of filtered scan points: 1290 points.
  4667. transformed_scan_ptr: 8177 points.
  4668. map: 6294 points.
  4669. NDT has converged: 1
  4670. Fitness score: 26.6652
  4671. Number of iteration: 3
  4672. (x,y,z,roll,pitch,yaw):
  4673. (0.0241409, -0.0152816, -0.0734753, 0.0128346, 0.00209277, -0.000503953)
  4674. Transformation Matrix:
  4675. 0.999998 0.000530775 0.00208612 0.0241409
  4676. -0.000503952 0.999918 -0.0128353 -0.0152816
  4677. -0.00209277 0.0128342 0.999915 -0.0734753
  4678. 0 0 0 1
  4679. shift: 0.0285712
  4680. -----------------------------------------------------------------
  4681. (Processed/Input): (246 / 246)
  4682. -----------------------------------------------------------------
  4683. Sequence number: 5176
  4684. Number of scan points: 8139 points.
  4685. Number of filtered scan points: 1294 points.
  4686. transformed_scan_ptr: 8139 points.
  4687. map: 6294 points.
  4688. NDT has converged: 1
  4689. Fitness score: 26.3578
  4690. Number of iteration: 3
  4691. (x,y,z,roll,pitch,yaw):
  4692. (0.00589906, -0.0528346, -0.0805056, 0.0145176, 0.00102763, -0.00695321)
  4693. Transformation Matrix:
  4694. 0.999975 0.00696734 0.000926565 0.00589906
  4695. -0.00695315 0.99987 -0.0145238 -0.0528346
  4696. -0.00102763 0.014517 0.999894 -0.0805056
  4697. 0 0 0 1
  4698. shift: 0.0531629
  4699. -----------------------------------------------------------------
  4700. (Processed/Input): (247 / 247)
  4701. -----------------------------------------------------------------
  4702. Sequence number: 5177
  4703. Number of scan points: 8173 points.
  4704. Number of filtered scan points: 1307 points.
  4705. transformed_scan_ptr: 8173 points.
  4706. map: 6294 points.
  4707. NDT has converged: 1
  4708. Fitness score: 25.5618
  4709. Number of iteration: 3
  4710. (x,y,z,roll,pitch,yaw):
  4711. (-0.0306689, -0.0537412, -0.0827513, 0.0119867, -0.000971984, -0.00866451)
  4712. Transformation Matrix:
  4713. 0.999962 0.00865213 -0.00107573 -0.0306689
  4714. -0.0086644 0.999891 -0.0119776 -0.0537412
  4715. 0.000971984 0.0119864 0.999928 -0.0827513
  4716. 0 0 0 1
  4717. shift: 0.0618765
  4718. -----------------------------------------------------------------
  4719. (Processed/Input): (248 / 248)
  4720. -----------------------------------------------------------------
  4721. Sequence number: 5178
  4722. Number of scan points: 8317 points.
  4723. Number of filtered scan points: 1344 points.
  4724. transformed_scan_ptr: 8317 points.
  4725. map: 6294 points.
  4726. NDT has converged: 1
  4727. Fitness score: 26.2897
  4728. Number of iteration: 2
  4729. (x,y,z,roll,pitch,yaw):
  4730. (-0.0288379, -0.0573513, -0.06627, 0.00307259, -0.00300983, -0.0123385)
  4731. Transformation Matrix:
  4732. 0.999919 0.0123289 -0.00304748 -0.0288379
  4733. -0.0123381 0.999919 -0.00303522 -0.0573513
  4734. 0.00300982 0.00307257 0.999991 -0.06627
  4735. 0 0 0 1
  4736. shift: 0.0641934
  4737. -----------------------------------------------------------------
  4738. (Processed/Input): (249 / 249)
  4739. -----------------------------------------------------------------
  4740. Sequence number: 5179
  4741. Number of scan points: 8173 points.
  4742. Number of filtered scan points: 1305 points.
  4743. transformed_scan_ptr: 8173 points.
  4744. map: 6294 points.
  4745. NDT has converged: 1
  4746. Fitness score: 25.2804
  4747. Number of iteration: 4
  4748. (x,y,z,roll,pitch,yaw):
  4749. (0.00356726, 0.00767195, -0.0521978, -0.00108635, -0.00342838, 0.00137834)
  4750. Transformation Matrix:
  4751. 0.999993 -0.00137462 -0.00342986 0.00356726
  4752. 0.00137833 0.999998 0.00108164 0.00767195
  4753. 0.00342837 -0.00108635 0.999994 -0.0521978
  4754. 0 0 0 1
  4755. shift: 0.00846074
  4756. -----------------------------------------------------------------
  4757. (Processed/Input): (250 / 250)
  4758. -----------------------------------------------------------------
  4759. Sequence number: 5180
  4760. Number of scan points: 8063 points.
  4761. Number of filtered scan points: 1278 points.
  4762. transformed_scan_ptr: 8063 points.
  4763. map: 6294 points.
  4764. NDT has converged: 1
  4765. Fitness score: 25.0512
  4766. Number of iteration: 5
  4767. (x,y,z,roll,pitch,yaw):
  4768. (-0.0204955, 0.0336291, -0.0407785, -0.00300271, -0.00275767, -0.0041844)
  4769. Transformation Matrix:
  4770. 0.999987 0.00419266 -0.00274505 -0.0204955
  4771. -0.00418437 0.999987 0.00301423 0.0336291
  4772. 0.00275767 -0.00300269 0.999992 -0.0407785
  4773. 0 0 0 1
  4774. shift: 0.0393825
  4775. -----------------------------------------------------------------
  4776. (Processed/Input): (251 / 251)
  4777. -----------------------------------------------------------------
  4778. Sequence number: 5181
  4779. Number of scan points: 8233 points.
  4780. Number of filtered scan points: 1308 points.
  4781. transformed_scan_ptr: 8233 points.
  4782. map: 6294 points.
  4783. NDT has converged: 1
  4784. Fitness score: 24.6911
  4785. Number of iteration: 2
  4786. (x,y,z,roll,pitch,yaw):
  4787. (-0.00992038, 0.0342593, -0.0454239, -0.00374402, -0.000553663, -0.00175667)
  4788. Transformation Matrix:
  4789. 0.999998 0.00175872 -0.000547085 -0.00992038
  4790. -0.00175666 0.999991 0.00374498 0.0342593
  4791. 0.000553663 -0.00374402 0.999993 -0.0454239
  4792. 0 0 0 1
  4793. shift: 0.0356667
  4794. -----------------------------------------------------------------
  4795. (Processed/Input): (252 / 252)
  4796. -----------------------------------------------------------------
  4797. Sequence number: 5182
  4798. Number of scan points: 8392 points.
  4799. Number of filtered scan points: 1338 points.
  4800. transformed_scan_ptr: 8392 points.
  4801. map: 6294 points.
  4802. NDT has converged: 1
  4803. Fitness score: 25.1692
  4804. Number of iteration: 3
  4805. (x,y,z,roll,pitch,yaw):
  4806. (0.0577895, 0.0351498, -0.0481134, -0.00219929, -0.00010404, -0.000298837)
  4807. Transformation Matrix:
  4808. 1 0.000299063 -0.00010339 0.0577895
  4809. -0.000298837 0.999998 0.00219932 0.0351498
  4810. 0.00010404 -0.00219929 0.999998 -0.0481134
  4811. 0 0 0 1
  4812. shift: 0.0676398
  4813. -----------------------------------------------------------------
  4814. (Processed/Input): (253 / 253)
  4815. -----------------------------------------------------------------
  4816. Sequence number: 5183
  4817. Number of scan points: 8364 points.
  4818. Number of filtered scan points: 1361 points.
  4819. transformed_scan_ptr: 8364 points.
  4820. map: 6294 points.
  4821. NDT has converged: 1
  4822. Fitness score: 25.2478
  4823. Number of iteration: 6
  4824. (x,y,z,roll,pitch,yaw):
  4825. (0.0144972, 0.0489172, -0.0462768, 0.00172379, -0.000361851, -0.00178123)
  4826. Transformation Matrix:
  4827. 0.999998 0.00178059 -0.000364942 0.0144972
  4828. -0.00178122 0.999997 -0.00172314 0.0489172
  4829. 0.000361851 0.00172379 0.999998 -0.0462768
  4830. 0 0 0 1
  4831. shift: 0.0510202
  4832. -----------------------------------------------------------------
  4833. (Processed/Input): (254 / 254)
  4834. -----------------------------------------------------------------
  4835. Sequence number: 5184
  4836. Number of scan points: 8232 points.
  4837. Number of filtered scan points: 1356 points.
  4838. transformed_scan_ptr: 8232 points.
  4839. map: 6294 points.
  4840. NDT has converged: 1
  4841. Fitness score: 25.6267
  4842. Number of iteration: 3
  4843. (x,y,z,roll,pitch,yaw):
  4844. (-0.0425802, 0.0176745, -0.0400915, 0.00240261, -0.00158298, -0.00779875)
  4845. Transformation Matrix:
  4846. 0.999968 0.00779485 -0.00160165 -0.0425802
  4847. -0.00779866 0.999967 -0.00239019 0.0176745
  4848. 0.00158298 0.0024026 0.999996 -0.0400915
  4849. 0 0 0 1
  4850. shift: 0.0461027
  4851. -----------------------------------------------------------------
  4852. (Processed/Input): (255 / 255)
  4853. -----------------------------------------------------------------
  4854. Sequence number: 5185
  4855. Number of scan points: 8247 points.
  4856. Number of filtered scan points: 1311 points.
  4857. transformed_scan_ptr: 8247 points.
  4858. map: 6294 points.
  4859. NDT has converged: 1
  4860. Fitness score: 24.8897
  4861. Number of iteration: 2
  4862. (x,y,z,roll,pitch,yaw):
  4863. (-0.0644669, -0.0121179, -0.0362896, 0.00336703, -0.00118184, -0.0118851)
  4864. Transformation Matrix:
  4865. 0.999929 0.0118807 -0.00122175 -0.0644669
  4866. -0.0118848 0.999924 -0.00335274 -0.0121179
  4867. 0.00118184 0.00336702 0.999994 -0.0362896
  4868. 0 0 0 1
  4869. shift: 0.065596
  4870. -----------------------------------------------------------------
  4871. (Processed/Input): (256 / 256)
  4872. -----------------------------------------------------------------
  4873. Sequence number: 5186
  4874. Number of scan points: 8194 points.
  4875. Number of filtered scan points: 1315 points.
  4876. transformed_scan_ptr: 8194 points.
  4877. map: 6294 points.
  4878. NDT has converged: 1
  4879. Fitness score: 25.0047
  4880. Number of iteration: 3
  4881. (x,y,z,roll,pitch,yaw):
  4882. (-0.0256498, -0.0167016, -0.0330139, 0.0044943, -0.000637245, -0.00958767)
  4883. Transformation Matrix:
  4884. 0.999954 0.00958456 -0.000680312 -0.0256498
  4885. -0.00958752 0.999944 -0.00448797 -0.0167016
  4886. 0.000637245 0.00449428 0.99999 -0.0330139
  4887. 0 0 0 1
  4888. shift: 0.0306081
  4889. -----------------------------------------------------------------
  4890. (Processed/Input): (257 / 257)
  4891. -----------------------------------------------------------------
  4892. Sequence number: 5187
  4893. Number of scan points: 8192 points.
  4894. Number of filtered scan points: 1332 points.
  4895. transformed_scan_ptr: 8192 points.
  4896. map: 6294 points.
  4897. NDT has converged: 1
  4898. Fitness score: 25.2607
  4899. Number of iteration: 4
  4900. (x,y,z,roll,pitch,yaw):
  4901. (0.0146399, 0.0386071, -0.031684, 0.00353561, 5.4223e-05, -0.00164152)
  4902. Transformation Matrix:
  4903. 0.999999 0.0016417 4.84413e-05 0.0146399
  4904. -0.00164152 0.999992 -0.00353569 0.0386071
  4905. -5.4223e-05 0.0035356 0.999994 -0.031684
  4906. 0 0 0 1
  4907. shift: 0.0412897
  4908. -----------------------------------------------------------------
  4909. (Processed/Input): (258 / 258)
  4910. -----------------------------------------------------------------
  4911. Sequence number: 5188
  4912. Number of scan points: 8081 points.
  4913. Number of filtered scan points: 1294 points.
  4914. transformed_scan_ptr: 8081 points.
  4915. map: 6294 points.
  4916. NDT has converged: 1
  4917. Fitness score: 25.0251
  4918. Number of iteration: 5
  4919. (x,y,z,roll,pitch,yaw):
  4920. (0.0103925, -0.046083, -0.0265728, 0.00527207, 0.00198461, -0.00473069)
  4921. Transformation Matrix:
  4922. 0.999987 0.00474107 0.00195963 0.0103925
  4923. -0.00473067 0.999975 -0.00528137 -0.046083
  4924. -0.0019846 0.00527204 0.999984 -0.0265728
  4925. 0 0 0 1
  4926. shift: 0.0472403
  4927. -----------------------------------------------------------------
  4928. (Processed/Input): (259 / 259)
  4929. -----------------------------------------------------------------
  4930. Sequence number: 5189
  4931. Number of scan points: 8047 points.
  4932. Number of filtered scan points: 1267 points.
  4933. transformed_scan_ptr: 8047 points.
  4934. map: 6294 points.
  4935. NDT has converged: 1
  4936. Fitness score: 24.6527
  4937. Number of iteration: 3
  4938. (x,y,z,roll,pitch,yaw):
  4939. (-0.00576676, -0.0723444, -0.0260248, 0.00412888, 0.00470133, 0.00113543)
  4940. Transformation Matrix:
  4941. 0.999988 -0.00111601 0.00470596 -0.00576676
  4942. 0.00113541 0.999991 -0.00412353 -0.0723444
  4943. -0.00470132 0.00412882 0.99998 -0.0260248
  4944. 0 0 0 1
  4945. shift: 0.0725739
  4946. -----------------------------------------------------------------
  4947. (Processed/Input): (260 / 260)
  4948. -----------------------------------------------------------------
  4949. Sequence number: 5190
  4950. Number of scan points: 8119 points.
  4951. Number of filtered scan points: 1298 points.
  4952. transformed_scan_ptr: 8119 points.
  4953. map: 6294 points.
  4954. NDT has converged: 1
  4955. Fitness score: 24.721
  4956. Number of iteration: 3
  4957. (x,y,z,roll,pitch,yaw):
  4958. (0.0361137, -0.0515989, -0.0271955, 0.00398641, 0.00760516, -0.00144583)
  4959. Transformation Matrix:
  4960. 0.99997 0.00147612 0.00759926 0.0361137
  4961. -0.00144579 0.999991 -0.00399738 -0.0515989
  4962. -0.00760509 0.00398628 0.999963 -0.0271955
  4963. 0 0 0 1
  4964. shift: 0.0629813
  4965. -----------------------------------------------------------------
  4966. (Processed/Input): (261 / 261)
  4967. -----------------------------------------------------------------
  4968. Sequence number: 5191
  4969. Number of scan points: 8044 points.
  4970. Number of filtered scan points: 1274 points.
  4971. transformed_scan_ptr: 8044 points.
  4972. map: 6294 points.
  4973. NDT has converged: 1
  4974. Fitness score: 23.8673
  4975. Number of iteration: 3
  4976. (x,y,z,roll,pitch,yaw):
  4977. (0.000984923, -0.055968, -0.0309574, 0.00335494, 0.00869182, -0.00395386)
  4978. Transformation Matrix:
  4979. 0.999954 0.00398298 0.00867834 0.000984923
  4980. -0.0039537 0.999986 -0.00338926 -0.055968
  4981. -0.00869171 0.0033548 0.999957 -0.0309574
  4982. 0 0 0 1
  4983. shift: 0.0559767
  4984. -----------------------------------------------------------------
  4985. (Processed/Input): (262 / 262)
  4986. -----------------------------------------------------------------
  4987. Sequence number: 5192
  4988. Number of scan points: 7975 points.
  4989. Number of filtered scan points: 1262 points.
  4990. transformed_scan_ptr: 7975 points.
  4991. map: 6294 points.
  4992. NDT has converged: 1
  4993. Fitness score: 23.8408
  4994. Number of iteration: 3
  4995. (x,y,z,roll,pitch,yaw):
  4996. (0.0019158, -0.0139872, -0.0322896, 0.00294, 0.00960963, -0.0128992)
  4997. Transformation Matrix:
  4998. 0.999871 0.0129271 0.00957072 0.0019158
  4999. -0.0128983 0.999912 -0.0030637 -0.0139872
  5000. -0.00960949 0.00293986 0.99995 -0.0322896
  5001. 0 0 0 1
  5002. shift: 0.0141177
  5003. -----------------------------------------------------------------
  5004. (Processed/Input): (263 / 263)
  5005. -----------------------------------------------------------------
  5006. Sequence number: 5193
  5007. Number of scan points: 8115 points.
  5008. Number of filtered scan points: 1282 points.
  5009. transformed_scan_ptr: 8115 points.
  5010. map: 6294 points.
  5011. NDT has converged: 1
  5012. Fitness score: 23.8555
  5013. Number of iteration: 5
  5014. (x,y,z,roll,pitch,yaw):
  5015. (0.0919221, 0.00592718, -0.0430206, 0.00253843, 0.00911776, -0.00753694)
  5016. Transformation Matrix:
  5017. 0.99993 0.00755998 0.00909822 0.0919221
  5018. -0.00753655 0.999968 -0.00260705 0.00592718
  5019. -0.00911763 0.00253832 0.999955 -0.0430206
  5020. 0 0 0 1
  5021. shift: 0.092113
  5022. -----------------------------------------------------------------
  5023. (Processed/Input): (264 / 264)
  5024. -----------------------------------------------------------------
  5025. Sequence number: 5194
  5026. Number of scan points: 7900 points.
  5027. Number of filtered scan points: 1273 points.
  5028. transformed_scan_ptr: 7900 points.
  5029. map: 6294 points.
  5030. NDT has converged: 1
  5031. Fitness score: 24.5335
  5032. Number of iteration: 5
  5033. (x,y,z,roll,pitch,yaw):
  5034. (0.0231745, -0.00327787, -0.0462627, 0.00633187, 0.00275582, 0.00504745)
  5035. Transformation Matrix:
  5036. 0.999983 -0.00502987 0.00278768 0.0231745
  5037. 0.00504741 0.999967 -0.00631784 -0.00327787
  5038. -0.00275581 0.00633181 0.999976 -0.0462627
  5039. 0 0 0 1
  5040. shift: 0.0234052
  5041. -----------------------------------------------------------------
  5042. (Processed/Input): (265 / 265)
  5043. -----------------------------------------------------------------
  5044. Sequence number: 5195
  5045. Number of scan points: 7794 points.
  5046. Number of filtered scan points: 1299 points.
  5047. transformed_scan_ptr: 7794 points.
  5048. map: 6294 points.
  5049. NDT has converged: 1
  5050. Fitness score: 25.1279
  5051. Number of iteration: 5
  5052. (x,y,z,roll,pitch,yaw):
  5053. (0.0311208, 0.00969823, -0.0509223, 0.00714743, -0.000149069, 2.96205e-06)
  5054. Transformation Matrix:
  5055. 1 -4.02811e-06 -0.000149044 0.0311208
  5056. 2.96205e-06 0.999974 -0.00714737 0.00969823
  5057. 0.000149069 0.00714737 0.999974 -0.0509223
  5058. 0 0 0 1
  5059. shift: 0.0325969
  5060. -----------------------------------------------------------------
  5061. (Processed/Input): (266 / 266)
  5062. -----------------------------------------------------------------
  5063. Sequence number: 5196
  5064. Number of scan points: 7864 points.
  5065. Number of filtered scan points: 1293 points.
  5066. transformed_scan_ptr: 7864 points.
  5067. map: 6294 points.
  5068. NDT has converged: 1
  5069. Fitness score: 25.6248
  5070. Number of iteration: 5
  5071. (x,y,z,roll,pitch,yaw):
  5072. (0.00794633, 0.0826549, -0.0458292, 0.00338802, 0.000116622, -0.0040635)
  5073. Transformation Matrix:
  5074. 0.999992 0.00406386 0.000102849 0.00794633
  5075. -0.00406349 0.999986 -0.00338846 0.0826549
  5076. -0.000116622 0.00338802 0.999994 -0.0458292
  5077. 0 0 0 1
  5078. shift: 0.083036
  5079. -----------------------------------------------------------------
  5080. (Processed/Input): (267 / 267)
  5081. -----------------------------------------------------------------
  5082. Sequence number: 5197
  5083. Number of scan points: 8103 points.
  5084. Number of filtered scan points: 1321 points.
  5085. transformed_scan_ptr: 8103 points.
  5086. map: 6294 points.
  5087. NDT has converged: 1
  5088. Fitness score: 25.6964
  5089. Number of iteration: 2
  5090. (x,y,z,roll,pitch,yaw):
  5091. (0.0040148, 0.121085, -0.0437799, -0.0023596, 0.00247364, -0.00658646)
  5092. Transformation Matrix:
  5093. 0.999975 0.00658056 0.00248913 0.0040148
  5094. -0.0065864 0.999976 0.00234324 0.121085
  5095. -0.00247364 -0.00235959 0.999994 -0.0437799
  5096. 0 0 0 1
  5097. shift: 0.121152
  5098. -----------------------------------------------------------------
  5099. (Processed/Input): (268 / 268)
  5100. -----------------------------------------------------------------
  5101. Sequence number: 5198
  5102. Number of scan points: 7709 points.
  5103. Number of filtered scan points: 1285 points.
  5104. transformed_scan_ptr: 7709 points.
  5105. map: 6294 points.
  5106. NDT has converged: 1
  5107. Fitness score: 25.7283
  5108. Number of iteration: 4
  5109. (x,y,z,roll,pitch,yaw):
  5110. (0.0444419, 0.0745009, -0.0382496, -0.00382427, 0.00447598, -0.000195127)
  5111. Transformation Matrix:
  5112. 0.99999 0.00017801 0.00447668 0.0444419
  5113. -0.000195125 0.999993 0.00382338 0.0745009
  5114. -0.00447597 -0.00382422 0.999983 -0.0382496
  5115. 0 0 0 1
  5116. shift: 0.0867494
  5117. -----------------------------------------------------------------
  5118. (Processed/Input): (269 / 269)
  5119. -----------------------------------------------------------------
  5120. Sequence number: 5199
  5121. Number of scan points: 7626 points.
  5122. Number of filtered scan points: 1276 points.
  5123. transformed_scan_ptr: 7626 points.
  5124. map: 6294 points.
  5125. NDT has converged: 1
  5126. Fitness score: 26.5915
  5127. Number of iteration: 3
  5128. (x,y,z,roll,pitch,yaw):
  5129. (0.0157397, 0.0448518, -0.0345262, -0.00475116, 0.00583836, -0.00417867)
  5130. Transformation Matrix:
  5131. 0.999974 0.00415087 0.00585807 0.0157397
  5132. -0.00417859 0.99998 0.0047267 0.0448518
  5133. -0.00583832 -0.00475106 0.999972 -0.0345262
  5134. 0 0 0 1
  5135. shift: 0.0475334
  5136. -----------------------------------------------------------------
  5137. (Processed/Input): (270 / 270)
  5138. -----------------------------------------------------------------
  5139. Sequence number: 5200
  5140. Number of scan points: 7685 points.
  5141. Number of filtered scan points: 1295 points.
  5142. transformed_scan_ptr: 7685 points.
  5143. map: 6294 points.
  5144. NDT has converged: 1
  5145. Fitness score: 26.8623
  5146. Number of iteration: 3
  5147. (x,y,z,roll,pitch,yaw):
  5148. (0.00976921, 0.0210253, -0.0408589, -0.00349459, 0.00769044, 0.00906748)
  5149. Transformation Matrix:
  5150. 0.999929 -0.00909417 0.00765832 0.00976921
  5151. 0.00906709 0.999953 0.00356415 0.0210253
  5152. -0.00769036 -0.00349448 0.999964 -0.0408589
  5153. 0 0 0 1
  5154. shift: 0.0231841
  5155. -----------------------------------------------------------------
  5156. (Processed/Input): (271 / 271)
  5157. -----------------------------------------------------------------
  5158. Sequence number: 5201
  5159. Number of scan points: 7502 points.
  5160. Number of filtered scan points: 1288 points.
  5161. transformed_scan_ptr: 7502 points.
  5162. map: 6294 points.
  5163. NDT has converged: 1
  5164. Fitness score: 27.4462
  5165. Number of iteration: 3
  5166. (x,y,z,roll,pitch,yaw):
  5167. (0.0613534, -0.0148195, -0.0447271, -0.0046208, 0.00923234, 0.0054512)
  5168. Transformation Matrix:
  5169. 0.999943 -0.00549376 0.0092068 0.0613534
  5170. 0.00545094 0.999974 0.00467103 -0.0148195
  5171. -0.00923221 -0.00462059 0.999947 -0.0447271
  5172. 0 0 0 1
  5173. shift: 0.0631178
  5174. -----------------------------------------------------------------
  5175. (Processed/Input): (272 / 272)
  5176. -----------------------------------------------------------------
  5177. Sequence number: 5202
  5178. Number of scan points: 7609 points.
  5179. Number of filtered scan points: 1283 points.
  5180. transformed_scan_ptr: 7609 points.
  5181. map: 6294 points.
  5182. NDT has converged: 1
  5183. Fitness score: 27.4595
  5184. Number of iteration: 3
  5185. (x,y,z,roll,pitch,yaw):
  5186. (0.0450273, -0.0481877, -0.0415594, -0.0030732, 0.0108039, -0.00264859)
  5187. Transformation Matrix:
  5188. 0.999938 0.00261539 0.0108117 0.0450273
  5189. -0.00264844 0.999992 0.00304458 -0.0481877
  5190. -0.0108037 -0.00307301 0.999937 -0.0415594
  5191. 0 0 0 1
  5192. shift: 0.0659508
  5193. -----------------------------------------------------------------
  5194. (Processed/Input): (273 / 273)
  5195. -----------------------------------------------------------------
  5196. Sequence number: 5203
  5197. Number of scan points: 7534 points.
  5198. Number of filtered scan points: 1265 points.
  5199. transformed_scan_ptr: 7534 points.
  5200. map: 6294 points.
  5201. NDT has converged: 1
  5202. Fitness score: 27.5023
  5203. Number of iteration: 3
  5204. (x,y,z,roll,pitch,yaw):
  5205. (0.020782, -0.0106422, -0.0451411, -0.00302318, 0.00669919, -0.010396)
  5206. Transformation Matrix:
  5207. 0.999924 0.0103755 0.00673018 0.020782
  5208. -0.0103955 0.999942 0.00295337 -0.0106422
  5209. -0.00669914 -0.00302311 0.999973 -0.0451411
  5210. 0 0 0 1
  5211. shift: 0.0233484
  5212. -----------------------------------------------------------------
  5213. (Processed/Input): (274 / 274)
  5214. -----------------------------------------------------------------
  5215. Sequence number: 5204
  5216. Number of scan points: 7335 points.
  5217. Number of filtered scan points: 1250 points.
  5218. transformed_scan_ptr: 7335 points.
  5219. map: 6294 points.
  5220. NDT has converged: 1
  5221. Fitness score: 28.2233
  5222. Number of iteration: 3
  5223. (x,y,z,roll,pitch,yaw):
  5224. (0.0251596, 0.00984729, -0.0375286, -0.00761023, 0.00389468, 0.00170164)
  5225. Transformation Matrix:
  5226. 0.999991 -0.00173124 0.00388159 0.0251596
  5227. 0.00170162 0.99997 0.00761678 0.00984729
  5228. -0.00389467 -0.0076101 0.999963 -0.0375286
  5229. 0 0 0 1
  5230. shift: 0.0270181
  5231. -----------------------------------------------------------------
  5232. (Processed/Input): (275 / 275)
  5233. -----------------------------------------------------------------
  5234. Sequence number: 5205
  5235. Number of scan points: 7346 points.
  5236. Number of filtered scan points: 1260 points.
  5237. transformed_scan_ptr: 7346 points.
  5238. map: 6294 points.
  5239. NDT has converged: 1
  5240. Fitness score: 27.9635
  5241. Number of iteration: 4
  5242. (x,y,z,roll,pitch,yaw):
  5243. (0.0132933, 0.0179975, -0.0348606, -0.0131705, 0.00208262, -0.000764769)
  5244. Transformation Matrix:
  5245. 0.999998 0.000737278 0.00209251 0.0132933
  5246. -0.000764767 0.999913 0.0131686 0.0179975
  5247. -0.00208262 -0.0131701 0.999911 -0.0348606
  5248. 0 0 0 1
  5249. shift: 0.0223746
  5250. -----------------------------------------------------------------
  5251. (Processed/Input): (276 / 276)
  5252. -----------------------------------------------------------------
  5253. Sequence number: 5206
  5254. Number of scan points: 7518 points.
  5255. Number of filtered scan points: 1275 points.
  5256. transformed_scan_ptr: 7518 points.
  5257. map: 6294 points.
  5258. NDT has converged: 1
  5259. Fitness score: 29.414
  5260. Number of iteration: 2
  5261. (x,y,z,roll,pitch,yaw):
  5262. (0.0180262, 0.0221229, -0.0400892, -0.0209328, 0.000651498, -0.00659177)
  5263. Transformation Matrix:
  5264. 0.999978 0.00657664 0.000789319 0.0180262
  5265. -0.00659172 0.999759 0.0209265 0.0221229
  5266. -0.000651498 -0.0209313 0.999781 -0.0400892
  5267. 0 0 0 1
  5268. shift: 0.0285371
  5269. -----------------------------------------------------------------
  5270. (Processed/Input): (277 / 277)
  5271. -----------------------------------------------------------------
  5272. Sequence number: 5207
  5273. Number of scan points: 7615 points.
  5274. Number of filtered scan points: 1318 points.
  5275. transformed_scan_ptr: 7615 points.
  5276. map: 6294 points.
  5277. NDT has converged: 1
  5278. Fitness score: 29.667
  5279. Number of iteration: 3
  5280. (x,y,z,roll,pitch,yaw):
  5281. (0.0480619, -0.00184465, -0.0417096, -0.0296509, 0.00199143, 0.00104824)
  5282. Transformation Matrix:
  5283. 0.999997 -0.00110683 0.00195947 0.0480619
  5284. 0.00104824 0.99956 0.0296486 -0.00184465
  5285. -0.00199143 -0.0296465 0.999558 -0.0417096
  5286. 0 0 0 1
  5287. shift: 0.0480973
  5288. -----------------------------------------------------------------
  5289. (Processed/Input): (278 / 278)
  5290. -----------------------------------------------------------------
  5291. Sequence number: 5208
  5292. Number of scan points: 7457 points.
  5293. Number of filtered scan points: 1299 points.
  5294. transformed_scan_ptr: 7457 points.
  5295. map: 6294 points.
  5296. NDT has converged: 1
  5297. Fitness score: 29.4216
  5298. Number of iteration: 3
  5299. (x,y,z,roll,pitch,yaw):
  5300. (0.0330728, 0.0445188, -0.0408633, -0.0402954, 0.00235962, -0.00549715)
  5301. Transformation Matrix:
  5302. 0.999982 0.0053976 0.00257913 0.0330728
  5303. -0.0054971 0.999174 0.0402709 0.0445188
  5304. -0.00235962 -0.0402844 0.999185 -0.0408633
  5305. 0 0 0 1
  5306. shift: 0.0554593
  5307. -----------------------------------------------------------------
  5308. (Processed/Input): (279 / 279)
  5309. -----------------------------------------------------------------
  5310. Sequence number: 5209
  5311. Number of scan points: 7530 points.
  5312. Number of filtered scan points: 1333 points.
  5313. transformed_scan_ptr: 7530 points.
  5314. map: 6294 points.
  5315. NDT has converged: 1
  5316. Fitness score: 29.9984
  5317. Number of iteration: 4
  5318. (x,y,z,roll,pitch,yaw):
  5319. (0.0594407, 0.0373375, -0.0454848, -0.0502114, 0.00546648, -0.00438769)
  5320. Transformation Matrix:
  5321. 0.999975 0.00410778 0.00567973 0.0594407
  5322. -0.00438761 0.998731 0.0501659 0.0373375
  5323. -0.00546645 -0.0501896 0.998725 -0.0454848
  5324. 0 0 0 1
  5325. shift: 0.0701946
  5326. -----------------------------------------------------------------
  5327. (Processed/Input): (280 / 280)
  5328. -----------------------------------------------------------------
  5329. Sequence number: 5210
  5330. Number of scan points: 7721 points.
  5331. Number of filtered scan points: 1350 points.
  5332. transformed_scan_ptr: 7721 points.
  5333. map: 6294 points.
  5334. NDT has converged: 1
  5335. Fitness score: 29.4257
  5336. Number of iteration: 2
  5337. (x,y,z,roll,pitch,yaw):
  5338. (0.074082, 0.0426988, -0.0644582, -0.0548392, 0.011305, -0.00907023)
  5339. Transformation Matrix:
  5340. 0.999895 0.00843686 0.0117844 0.074082
  5341. -0.00906952 0.998461 0.0547071 0.0426988
  5342. -0.0113047 -0.0548082 0.998433 -0.0644582
  5343. 0 0 0 1
  5344. shift: 0.0855063
  5345. -----------------------------------------------------------------
  5346. (Processed/Input): (281 / 281)
  5347. -----------------------------------------------------------------
  5348. Sequence number: 5211
  5349. Number of scan points: 7429 points.
  5350. Number of filtered scan points: 1354 points.
  5351. transformed_scan_ptr: 7429 points.
  5352. map: 6294 points.
  5353. NDT has converged: 1
  5354. Fitness score: 29.4294
  5355. Number of iteration: 2
  5356. (x,y,z,roll,pitch,yaw):
  5357. (0.0896882, 0.0342609, -0.0803806, -0.0555882, 0.0155743, -0.00499917)
  5358. Transformation Matrix:
  5359. 0.999866 0.00412617 0.0158271 0.0896882
  5360. -0.00499854 0.998447 0.0554812 0.0342609
  5361. -0.0155736 -0.0555529 0.998334 -0.0803806
  5362. 0 0 0 1
  5363. shift: 0.0960093
  5364. -----------------------------------------------------------------
  5365. (Processed/Input): (282 / 282)
  5366. -----------------------------------------------------------------
  5367. Sequence number: 5212
  5368. Number of scan points: 7425 points.
  5369. Number of filtered scan points: 1357 points.
  5370. transformed_scan_ptr: 7425 points.
  5371. map: 6294 points.
  5372. NDT has converged: 1
  5373. Fitness score: 29.5895
  5374. Number of iteration: 3
  5375. (x,y,z,roll,pitch,yaw):
  5376. (0.0849744, 0.061929, -0.111862, -0.0481086, 0.0177627, -0.00499957)
  5377. Transformation Matrix:
  5378. 0.99983 0.00413961 0.0179814 0.0849744
  5379. -0.00499876 0.998835 0.0480007 0.061929
  5380. -0.0177618 -0.0480824 0.998685 -0.111862
  5381. 0 0 0 1
  5382. shift: 0.105147
  5383. -----------------------------------------------------------------
  5384. (Processed/Input): (283 / 283)
  5385. -----------------------------------------------------------------
  5386. Sequence number: 5213
  5387. Number of scan points: 7280 points.
  5388. Number of filtered scan points: 1306 points.
  5389. transformed_scan_ptr: 7280 points.
  5390. map: 6294 points.
  5391. NDT has converged: 1
  5392. Fitness score: 29.7408
  5393. Number of iteration: 3
  5394. (x,y,z,roll,pitch,yaw):
  5395. (0.0187874, 0.0892031, -0.145284, -0.0317471, 0.0177606, 0.00244836)
  5396. Transformation Matrix:
  5397. 0.999839 -0.00301083 0.017673 0.0187874
  5398. 0.00244797 0.999492 0.0317851 0.0892031
  5399. -0.0177597 -0.0317367 0.999339 -0.145284
  5400. 0 0 0 1
  5401. shift: 0.0911601
  5402. -----------------------------------------------------------------
  5403. (Processed/Input): (284 / 284)
  5404. -----------------------------------------------------------------
  5405. Sequence number: 5214
  5406. Number of scan points: 7112 points.
  5407. Number of filtered scan points: 1276 points.
  5408. transformed_scan_ptr: 7112 points.
  5409. map: 6294 points.
  5410. NDT has converged: 1
  5411. Fitness score: 30.1131
  5412. Number of iteration: 5
  5413. (x,y,z,roll,pitch,yaw):
  5414. (0.0829025, -0.00111748, -0.177522, -0.00897069, 0.0200096, -0.00466185)
  5415. Transformation Matrix:
  5416. 0.999789 0.00448215 0.0200491 0.0829025
  5417. -0.0046609 0.99995 0.00887719 -0.00111748
  5418. -0.0200083 -0.00896877 0.99976 -0.177522
  5419. 0 0 0 1
  5420. shift: 0.08291
  5421. -----------------------------------------------------------------
  5422. (Processed/Input): (285 / 285)
  5423. -----------------------------------------------------------------
  5424. Sequence number: 5215
  5425. Number of scan points: 7075 points.
  5426. Number of filtered scan points: 1240 points.
  5427. transformed_scan_ptr: 7075 points.
  5428. map: 6294 points.
  5429. NDT has converged: 1
  5430. Fitness score: 30.6658
  5431. Number of iteration: 5
  5432. (x,y,z,roll,pitch,yaw):
  5433. (0.0794521, -0.0825648, -0.205781, 0.0155484, 0.0190271, 0.000352682)
  5434. Transformation Matrix:
  5435. 0.999819 -5.68132e-05 0.0190291 0.0794521
  5436. 0.000352618 0.999879 -0.0155411 -0.0825648
  5437. -0.0190259 0.015545 0.999698 -0.205781
  5438. 0 0 0 1
  5439. shift: 0.114584
  5440. -----------------------------------------------------------------
  5441. (Processed/Input): (286 / 286)
  5442. -----------------------------------------------------------------
  5443. Sequence number: 5216
  5444. Number of scan points: 6598 points.
  5445. Number of filtered scan points: 1215 points.
  5446. transformed_scan_ptr: 6598 points.
  5447. map: 6294 points.
  5448. NDT has converged: 1
  5449. Fitness score: 31.1893
  5450. Number of iteration: 6
  5451. (x,y,z,roll,pitch,yaw):
  5452. (0.0188158, -0.106819, -0.223296, 0.0371271, 0.0162445, -0.0119562)
  5453. Transformation Matrix:
  5454. 0.999797 0.0125506 0.0157877 0.0188158
  5455. -0.0119543 0.999232 -0.03731 -0.106819
  5456. -0.0162438 0.0371137 0.999179 -0.223296
  5457. 0 0 0 1
  5458. shift: 0.108464
  5459. -----------------------------------------------------------------
  5460. (Processed/Input): (287 / 287)
  5461. -----------------------------------------------------------------
  5462. Sequence number: 5217
  5463. Number of scan points: 6231 points.
  5464. Number of filtered scan points: 1129 points.
  5465. transformed_scan_ptr: 6231 points.
  5466. map: 6294 points.
  5467. NDT has converged: 1
  5468. Fitness score: 29.5734
  5469. Number of iteration: 4
  5470. (x,y,z,roll,pitch,yaw):
  5471. (-0.0374703, -0.15004, -0.251102, 0.0539887, 0.00853839, -0.014458)
  5472. Transformation Matrix:
  5473. 0.999859 0.0148971 0.00774479 -0.0374703
  5474. -0.014457 0.998432 -0.0540801 -0.15004
  5475. -0.00853828 0.0539605 0.998507 -0.251102
  5476. 0 0 0 1
  5477. shift: 0.154648
  5478. -----------------------------------------------------------------
  5479. (Processed/Input): (288 / 288)
  5480. -----------------------------------------------------------------
  5481. Sequence number: 5218
  5482. Number of scan points: 6292 points.
  5483. Number of filtered scan points: 1121 points.
  5484. transformed_scan_ptr: 6292 points.
  5485. map: 6294 points.
  5486. NDT has converged: 1
  5487. Fitness score: 29.7399
  5488. Number of iteration: 8
  5489. (x,y,z,roll,pitch,yaw):
  5490. (0.0450248, -0.172411, -0.266643, 0.0634236, -0.00416366, -0.00517718)
  5491. Transformation Matrix:
  5492. 0.999978 0.00490286 -0.00448336 0.0450248
  5493. -0.00517712 0.997977 -0.0633587 -0.172411
  5494. 0.00416365 0.0633805 0.997981 -0.266643
  5495. 0 0 0 1
  5496. shift: 0.178193
  5497. -----------------------------------------------------------------
  5498. (Processed/Input): (289 / 289)
  5499. -----------------------------------------------------------------
  5500. Sequence number: 5219
  5501. Number of scan points: 6302 points.
  5502. Number of filtered scan points: 1136 points.
  5503. transformed_scan_ptr: 6302 points.
  5504. map: 6294 points.
  5505. NDT has converged: 1
  5506. Fitness score: 29.6371
  5507. Number of iteration: 6
  5508. (x,y,z,roll,pitch,yaw):
  5509. (-0.00986744, -0.220704, -0.287239, 0.0696216, -0.00996268, -0.00626197)
  5510. Transformation Matrix:
  5511. 0.999931 0.00555373 -0.0103738 -0.00986744
  5512. -0.00626162 0.997562 -0.0695018 -0.220704
  5513. 0.00996252 0.0695619 0.997528 -0.287239
  5514. 0 0 0 1
  5515. shift: 0.220925
  5516. -----------------------------------------------------------------
  5517. (Processed/Input): (290 / 290)
  5518. -----------------------------------------------------------------
  5519. Sequence number: 5220
  5520. Number of scan points: 6525 points.
  5521. Number of filtered scan points: 1119 points.
  5522. transformed_scan_ptr: 6525 points.
  5523. map: 6294 points.
  5524. NDT has converged: 1
  5525. Fitness score: 29.354
  5526. Number of iteration: 4
  5527. (x,y,z,roll,pitch,yaw):
  5528. (0.00623271, -0.207391, -0.286189, 0.072677, -0.0141156, -0.00339221)
  5529. Transformation Matrix:
  5530. 0.999895 0.00235832 -0.0143241 0.00623271
  5531. -0.00339187 0.997358 -0.0725649 -0.207391
  5532. 0.0141151 0.0726058 0.997261 -0.286189
  5533. 0 0 0 1
  5534. shift: 0.207485
  5535. -----------------------------------------------------------------
  5536. (Processed/Input): (291 / 291)
  5537. -----------------------------------------------------------------
  5538. Sequence number: 5221
  5539. Number of scan points: 6747 points.
  5540. Number of filtered scan points: 1154 points.
  5541. transformed_scan_ptr: 6747 points.
  5542. map: 6294 points.
  5543. NDT has converged: 1
  5544. Fitness score: 29.7126
  5545. Number of iteration: 4
  5546. (x,y,z,roll,pitch,yaw):
  5547. (-0.0443824, -0.171579, -0.260666, 0.0761553, -0.0184386, -0.00719969)
  5548. Transformation Matrix:
  5549. 0.999804 0.00577602 -0.0189314 -0.0443824
  5550. -0.0071984 0.997086 -0.0759474 -0.171579
  5551. 0.0184376 0.0760688 0.996932 -0.260666
  5552. 0 0 0 1
  5553. shift: 0.177226
  5554. -----------------------------------------------------------------
  5555. (Processed/Input): (292 / 292)
  5556. -----------------------------------------------------------------
  5557. Sequence number: 5222
  5558. Number of scan points: 6874 points.
  5559. Number of filtered scan points: 1189 points.
  5560. transformed_scan_ptr: 6874 points.
  5561. map: 6294 points.
  5562. NDT has converged: 1
  5563. Fitness score: 29.7991
  5564. Number of iteration: 3
  5565. (x,y,z,roll,pitch,yaw):
  5566. (-0.0718663, -0.165914, -0.254886, 0.0834485, -0.0243117, -0.0111599)
  5567. Transformation Matrix:
  5568. 0.999642 0.00909476 -0.0251534 -0.0718663
  5569. -0.0111564 0.996481 -0.0830762 -0.165914
  5570. 0.0243093 0.0833271 0.996226 -0.254886
  5571. 0 0 0 1
  5572. shift: 0.18081
  5573. -----------------------------------------------------------------
  5574. (Processed/Input): (293 / 293)
  5575. -----------------------------------------------------------------
  5576. Sequence number: 5223
  5577. Number of scan points: 6809 points.
  5578. Number of filtered scan points: 1148 points.
  5579. transformed_scan_ptr: 6809 points.
  5580. map: 6294 points.
  5581. NDT has converged: 1
  5582. Fitness score: 28.1487
  5583. Number of iteration: 3
  5584. (x,y,z,roll,pitch,yaw):
  5585. (-0.0701424, -0.217803, -0.19713, 0.0866527, -0.0226808, -0.00590817)
  5586. Transformation Matrix:
  5587. 0.999725 0.00392327 -0.0231047 -0.0701424
  5588. -0.00590661 0.996242 -0.0864093 -0.217803
  5589. 0.0226788 0.0865221 0.995992 -0.19713
  5590. 0 0 0 1
  5591. shift: 0.228819
  5592. -----------------------------------------------------------------
  5593. (Processed/Input): (294 / 294)
  5594. -----------------------------------------------------------------
  5595. Sequence number: 5224
  5596. Number of scan points: 7021 points.
  5597. Number of filtered scan points: 1184 points.
  5598. transformed_scan_ptr: 7021 points.
  5599. map: 6294 points.
  5600. NDT has converged: 1
  5601. Fitness score: 28.3907
  5602. Number of iteration: 3
  5603. (x,y,z,roll,pitch,yaw):
  5604. (0.0154584, -0.242735, -0.148632, 0.0936467, -0.019262, 0.00246463)
  5605. Transformation Matrix:
  5606. 0.999811 -0.00425489 -0.0189459 0.0154584
  5607. 0.00246417 0.995611 -0.0935569 -0.242735
  5608. 0.0192608 0.0934926 0.995434 -0.148632
  5609. 0 0 0 1
  5610. shift: 0.243227
  5611. -----------------------------------------------------------------
  5612. (Processed/Input): (295 / 295)
  5613. -----------------------------------------------------------------
  5614. Sequence number: 5225
  5615. Number of scan points: 7126 points.
  5616. Number of filtered scan points: 1191 points.
  5617. transformed_scan_ptr: 7126 points.
  5618. map: 6294 points.
  5619. NDT has converged: 1
  5620. Fitness score: 28.266
  5621. Number of iteration: 5
  5622. (x,y,z,roll,pitch,yaw):
  5623. (0.0103709, -0.272929, -0.104992, 0.0947741, -0.0117916, -0.00211743)
  5624. Transformation Matrix:
  5625. 0.999928 0.000992087 -0.0119387 0.0103709
  5626. -0.00211728 0.995512 -0.0946072 -0.272929
  5627. 0.0117913 0.0946257 0.995443 -0.104992
  5628. 0 0 0 1
  5629. shift: 0.273126
  5630. -----------------------------------------------------------------
  5631. (Processed/Input): (296 / 296)
  5632. -----------------------------------------------------------------
  5633. Sequence number: 5226
  5634. Number of scan points: 7169 points.
  5635. Number of filtered scan points: 1216 points.
  5636. transformed_scan_ptr: 7169 points.
  5637. map: 6294 points.
  5638. NDT has converged: 1
  5639. Fitness score: 28.2673
  5640. Number of iteration: 2
  5641. (x,y,z,roll,pitch,yaw):
  5642. (0.0271334, -0.264578, -0.0669092, 0.0967198, -0.00428659, -0.00290891)
  5643. Transformation Matrix:
  5644. 0.999987 0.00248137 -0.00454743 0.0271334
  5645. -0.00290888 0.995323 -0.0965563 -0.264578
  5646. 0.00428657 0.0965682 0.995317 -0.0669092
  5647. 0 0 0 1
  5648. shift: 0.265966
  5649. -----------------------------------------------------------------
  5650. (Processed/Input): (297 / 297)
  5651. -----------------------------------------------------------------
  5652. Sequence number: 5227
  5653. Number of scan points: 7078 points.
  5654. Number of filtered scan points: 1202 points.
  5655. transformed_scan_ptr: 7078 points.
  5656. map: 6294 points.
  5657. NDT has converged: 1
  5658. Fitness score: 28.3573
  5659. Number of iteration: 2
  5660. (x,y,z,roll,pitch,yaw):
  5661. (0.0643628, -0.254807, -0.043885, 0.0971177, 0.00251737, -0.00426886)
  5662. Transformation Matrix:
  5663. 0.999988 0.00449283 0.00209155 0.0643628
  5664. -0.00426883 0.995278 -0.0969749 -0.254807
  5665. -0.00251736 0.0969648 0.995285 -0.043885
  5666. 0 0 0 1
  5667. shift: 0.26281
  5668. -----------------------------------------------------------------
  5669. (Processed/Input): (298 / 298)
  5670. -----------------------------------------------------------------
  5671. Sequence number: 5228
  5672. Number of scan points: 7004 points.
  5673. Number of filtered scan points: 1208 points.
  5674. transformed_scan_ptr: 7004 points.
  5675. map: 6294 points.
  5676. NDT has converged: 1
  5677. Fitness score: 28.4701
  5678. Number of iteration: 2
  5679. (x,y,z,roll,pitch,yaw):
  5680. (0.098718, -0.224351, -0.0373852, 0.0929958, 0.00565675, -0.00513057)
  5681. Transformation Matrix:
  5682. 0.999971 0.00563366 0.00515577 0.098718
  5683. -0.00513046 0.995663 -0.0928895 -0.224351
  5684. -0.00565672 0.0928603 0.995663 -0.0373852
  5685. 0 0 0 1
  5686. shift: 0.24511
  5687. -----------------------------------------------------------------
  5688. (Processed/Input): (299 / 299)
  5689. -----------------------------------------------------------------
  5690. Sequence number: 5229
  5691. Number of scan points: 7069 points.
  5692. Number of filtered scan points: 1192 points.
  5693. transformed_scan_ptr: 7069 points.
  5694. map: 6294 points.
  5695. NDT has converged: 1
  5696. Fitness score: 28.3271
  5697. Number of iteration: 3
  5698. (x,y,z,roll,pitch,yaw):
  5699. (0.0624318, -0.21861, -0.0366114, 0.0865338, 0.00456071, 0.00168465)
  5700. Transformation Matrix:
  5701. 0.999988 -0.00128418 0.00468922 0.0624318
  5702. 0.00168463 0.996258 -0.086418 -0.21861
  5703. -0.00456069 0.0864249 0.996248 -0.0366114
  5704. 0 0 0 1
  5705. shift: 0.22735
  5706. -----------------------------------------------------------------
  5707. (Processed/Input): (300 / 300)
  5708. -----------------------------------------------------------------
  5709. Sequence number: 5230
  5710. Number of scan points: 7124 points.
  5711. Number of filtered scan points: 1197 points.
  5712. transformed_scan_ptr: 7124 points.
  5713. map: 6294 points.
  5714. NDT has converged: 1
  5715. Fitness score: 28.4595
  5716. Number of iteration: 2
  5717. (x,y,z,roll,pitch,yaw):
  5718. (0.0362716, -0.196201, -0.0384292, 0.0764881, 0.00294635, -0.00269447)
  5719. Transformation Matrix:
  5720. 0.999992 0.00291173 0.00273183 0.0362716
  5721. -0.00269446 0.997072 -0.0764211 -0.196201
  5722. -0.00294635 0.0764132 0.997072 -0.0384292
  5723. 0 0 0 1
  5724. shift: 0.199526
  5725. -----------------------------------------------------------------
  5726. (Processed/Input): (301 / 301)
  5727. -----------------------------------------------------------------
  5728. Sequence number: 5231
  5729. Number of scan points: 7105 points.
  5730. Number of filtered scan points: 1183 points.
  5731. transformed_scan_ptr: 7105 points.
  5732. map: 6294 points.
  5733. NDT has converged: 1
  5734. Fitness score: 28.4081
  5735. Number of iteration: 4
  5736. (x,y,z,roll,pitch,yaw):
  5737. (0.0364547, -0.197299, -0.0367511, 0.0645573, 0.0011334, 0.00129405)
  5738. Transformation Matrix:
  5739. 0.999999 -0.00121823 0.00121452 0.0364547
  5740. 0.00129405 0.997916 -0.0645109 -0.197299
  5741. -0.0011334 0.0645124 0.997916 -0.0367511
  5742. 0 0 0 1
  5743. shift: 0.200638
  5744. -----------------------------------------------------------------
  5745. (Processed/Input): (302 / 302)
  5746. -----------------------------------------------------------------
  5747. Sequence number: 5232
  5748. Number of scan points: 7082 points.
  5749. Number of filtered scan points: 1180 points.
  5750. transformed_scan_ptr: 7082 points.
  5751. map: 6294 points.
  5752. NDT has converged: 1
  5753. Fitness score: 28.096
  5754. Number of iteration: 3
  5755. (x,y,z,roll,pitch,yaw):
  5756. (0.00259166, -0.130443, -0.033699, 0.0556646, 0.0004552, -0.0086298)
  5757. Transformation Matrix:
  5758. 0.999963 0.00864165 -2.56493e-05 0.00259166
  5759. -0.00862969 0.998414 -0.0556377 -0.130443
  5760. -0.0004552 0.0556359 0.998451 -0.033699
  5761. 0 0 0 1
  5762. shift: 0.130469
  5763. -----------------------------------------------------------------
  5764. (Processed/Input): (303 / 303)
  5765. -----------------------------------------------------------------
  5766. Sequence number: 5233
  5767. Number of scan points: 6970 points.
  5768. Number of filtered scan points: 1167 points.
  5769. transformed_scan_ptr: 6970 points.
  5770. map: 6294 points.
  5771. NDT has converged: 1
  5772. Fitness score: 28.4075
  5773. Number of iteration: 3
  5774. (x,y,z,roll,pitch,yaw):
  5775. (0.00628604, -0.119205, -0.0273693, 0.0498128, -0.000304946, -0.000524159)
  5776. Transformation Matrix:
  5777. 1 0.000508325 -0.000330667 0.00628604
  5778. -0.000524159 0.998759 -0.0497921 -0.119205
  5779. 0.000304946 0.0497922 0.99876 -0.0273693
  5780. 0 0 0 1
  5781. shift: 0.119371
  5782. -----------------------------------------------------------------
  5783. (Processed/Input): (304 / 304)
  5784. -----------------------------------------------------------------
  5785. Sequence number: 5234
  5786. Number of scan points: 7055 points.
  5787. Number of filtered scan points: 1166 points.
  5788. transformed_scan_ptr: 7055 points.
  5789. map: 6294 points.
  5790. NDT has converged: 1
  5791. Fitness score: 28.9478
  5792. Number of iteration: 3
  5793. (x,y,z,roll,pitch,yaw):
  5794. (0.069382, -0.110639, -0.0188778, 0.0436968, -0.00111402, 0.00583312)
  5795. Transformation Matrix:
  5796. 0.999982 -0.00587618 -0.000858124 0.069382
  5797. 0.00583309 0.999028 -0.0436886 -0.110639
  5798. 0.00111402 0.0436829 0.999045 -0.0188778
  5799. 0 0 0 1
  5800. shift: 0.130594
  5801. -----------------------------------------------------------------
  5802. (Processed/Input): (305 / 305)
  5803. -----------------------------------------------------------------
  5804. Sequence number: 5235
  5805. Number of scan points: 6996 points.
  5806. Number of filtered scan points: 1163 points.
  5807. transformed_scan_ptr: 6996 points.
  5808. map: 6294 points.
  5809. NDT has converged: 1
  5810. Fitness score: 28.159
  5811. Number of iteration: 4
  5812. (x,y,z,roll,pitch,yaw):
  5813. (0.0277672, -0.101208, -0.0108099, 0.0364921, -0.00385145, -0.00209652)
  5814. Transformation Matrix:
  5815. 0.99999 0.00195462 -0.00392536 0.0277672
  5816. -0.00209651 0.999332 -0.0364759 -0.101208
  5817. 0.00385144 0.0364837 0.999327 -0.0108099
  5818. 0 0 0 1
  5819. shift: 0.104948
  5820. -----------------------------------------------------------------
  5821. (Processed/Input): (306 / 306)
  5822. -----------------------------------------------------------------
  5823. Sequence number: 5236
  5824. Number of scan points: 7061 points.
  5825. Number of filtered scan points: 1180 points.
  5826. transformed_scan_ptr: 7061 points.
  5827. map: 6294 points.
  5828. NDT has converged: 1
  5829. Fitness score: 28.7375
  5830. Number of iteration: 2
  5831. (x,y,z,roll,pitch,yaw):
  5832. (0.0185314, -0.0768396, -0.00638643, 0.0309048, -0.00465081, -0.00324108)
  5833. Transformation Matrix:
  5834. 0.999984 0.00309582 -0.0047487 0.0185314
  5835. -0.00324104 0.999518 -0.0308847 -0.0768396
  5836. 0.0046508 0.0308995 0.999512 -0.00638643
  5837. 0 0 0 1
  5838. shift: 0.0790426
  5839. -----------------------------------------------------------------
  5840. (Processed/Input): (307 / 307)
  5841. -----------------------------------------------------------------
  5842. Sequence number: 5237
  5843. Number of scan points: 7014 points.
  5844. Number of filtered scan points: 1171 points.
  5845. transformed_scan_ptr: 7014 points.
  5846. map: 6294 points.
  5847. NDT has converged: 1
  5848. Fitness score: 28.8789
  5849. Number of iteration: 2
  5850. (x,y,z,roll,pitch,yaw):
  5851. (0.0590202, -0.0460478, 0.00267178, 0.0201747, -0.00473317, 0.00145237)
  5852. Transformation Matrix:
  5853. 0.999988 -0.00154755 -0.00470288 0.0590202
  5854. 0.00145235 0.999795 -0.0201802 -0.0460478
  5855. 0.00473315 0.0201731 0.999785 0.00267178
  5856. 0 0 0 1
  5857. shift: 0.0748584
  5858. -----------------------------------------------------------------
  5859. (Processed/Input): (308 / 308)
  5860. -----------------------------------------------------------------
  5861. Sequence number: 5238
  5862. Number of scan points: 7092 points.
  5863. Number of filtered scan points: 1188 points.
  5864. transformed_scan_ptr: 7092 points.
  5865. map: 6294 points.
  5866. NDT has converged: 1
  5867. Fitness score: 29.664
  5868. Number of iteration: 4
  5869. (x,y,z,roll,pitch,yaw):
  5870. (0.0319941, -0.0805672, 0.00793775, 0.0123359, -0.00158777, -0.000305139)
  5871. Transformation Matrix:
  5872. 0.999999 0.000285536 -0.00159141 0.0319941
  5873. -0.000305139 0.999924 -0.0123351 -0.0805672
  5874. 0.00158777 0.0123356 0.999923 0.00793775
  5875. 0 0 0 1
  5876. shift: 0.0866873
  5877. -----------------------------------------------------------------
  5878. (Processed/Input): (309 / 309)
  5879. -----------------------------------------------------------------
  5880. Sequence number: 5239
  5881. Number of scan points: 6974 points.
  5882. Number of filtered scan points: 1168 points.
  5883. transformed_scan_ptr: 6974 points.
  5884. map: 6294 points.
  5885. NDT has converged: 1
  5886. Fitness score: 28.6542
  5887. Number of iteration: 2
  5888. (x,y,z,roll,pitch,yaw):
  5889. (0.0123744, -0.0468715, 0.0107983, 0.0102675, 0.00224112, -0.000215196)
  5890. Transformation Matrix:
  5891. 0.999997 0.000238198 0.00223879 0.0123744
  5892. -0.000215196 0.999947 -0.0102678 -0.0468715
  5893. -0.00224112 0.0102673 0.999945 0.0107983
  5894. 0 0 0 1
  5895. shift: 0.0484774
  5896. -----------------------------------------------------------------
  5897. (Processed/Input): (310 / 310)
  5898. -----------------------------------------------------------------
  5899. Sequence number: 5240
  5900. Number of scan points: 7057 points.
  5901. Number of filtered scan points: 1181 points.
  5902. transformed_scan_ptr: 7057 points.
  5903. map: 6294 points.
  5904. NDT has converged: 1
  5905. Fitness score: 29.2852
  5906. Number of iteration: 2
  5907. (x,y,z,roll,pitch,yaw):
  5908. (0.0394357, -0.0397418, 0.00997557, 0.00655165, 0.00524195, -0.00336802)
  5909. Transformation Matrix:
  5910. 0.999981 0.0034023 0.00521971 0.0394357
  5911. -0.00336797 0.999973 -0.00656923 -0.0397418
  5912. -0.00524193 0.00655151 0.999965 0.00997557
  5913. 0 0 0 1
  5914. shift: 0.0559873
  5915. -----------------------------------------------------------------
  5916. (Processed/Input): (311 / 311)
  5917. -----------------------------------------------------------------
  5918. Sequence number: 5241
  5919. Number of scan points: 7086 points.
  5920. Number of filtered scan points: 1155 points.
  5921. transformed_scan_ptr: 7086 points.
  5922. map: 6294 points.
  5923. NDT has converged: 1
  5924. Fitness score: 29.5982
  5925. Number of iteration: 2
  5926. (x,y,z,roll,pitch,yaw):
  5927. (0.0448813, -0.0437834, 0.00427117, 0.00952466, 0.00471563, -0.00118483)
  5928. Transformation Matrix:
  5929. 0.999988 0.0012297 0.0047041 0.0448813
  5930. -0.00118481 0.999954 -0.00953009 -0.0437834
  5931. -0.00471561 0.00952441 0.999943 0.00427117
  5932. 0 0 0 1
  5933. shift: 0.0627002
  5934. -----------------------------------------------------------------
  5935. (Processed/Input): (312 / 312)
  5936. -----------------------------------------------------------------
  5937. Sequence number: 5242
  5938. Number of scan points: 7040 points.
  5939. Number of filtered scan points: 1155 points.
  5940. transformed_scan_ptr: 7040 points.
  5941. map: 6294 points.
  5942. NDT has converged: 1
  5943. Fitness score: 28.2579
  5944. Number of iteration: 2
  5945. (x,y,z,roll,pitch,yaw):
  5946. (0.0478798, -0.0736286, -0.00172364, 0.0187991, 0.00343542, -0.0021401)
  5947. Transformation Matrix:
  5948. 0.999992 0.0022043 0.00339457 0.0478798
  5949. -0.00214009 0.999821 -0.0188053 -0.0736286
  5950. -0.00343542 0.0187979 0.999817 -0.00172364
  5951. 0 0 0 1
  5952. shift: 0.0878273
  5953. -----------------------------------------------------------------
  5954. (Processed/Input): (313 / 313)
  5955. -----------------------------------------------------------------
  5956. Sequence number: 5243
  5957. Number of scan points: 6914 points.
  5958. Number of filtered scan points: 1134 points.
  5959. transformed_scan_ptr: 6914 points.
  5960. map: 6294 points.
  5961. NDT has converged: 1
  5962. Fitness score: 28.0426
  5963. Number of iteration: 2
  5964. (x,y,z,roll,pitch,yaw):
  5965. (0.0602234, -0.124521, -0.00706659, 0.0289744, 0.00193603, 0.000848024)
  5966. Transformation Matrix:
  5967. 0.999998 -0.00079158 0.00195979 0.0602234
  5968. 0.000848022 0.99958 -0.0289687 -0.124521
  5969. -0.00193603 0.0289703 0.999578 -0.00706659
  5970. 0 0 0 1
  5971. shift: 0.13832
  5972. -----------------------------------------------------------------
  5973. (Processed/Input): (314 / 314)
  5974. -----------------------------------------------------------------
  5975. Sequence number: 5244
  5976. Number of scan points: 7030 points.
  5977. Number of filtered scan points: 1162 points.
  5978. transformed_scan_ptr: 7030 points.
  5979. map: 6294 points.
  5980. NDT has converged: 1
  5981. Fitness score: 29.0454
  5982. Number of iteration: 3
  5983. (x,y,z,roll,pitch,yaw):
  5984. (0.0195744, -0.0553842, -0.0142108, 0.043199, 0.000416312, 0.00139002)
  5985. Transformation Matrix:
  5986. 0.999999 -0.00137075 0.00047595 0.0195744
  5987. 0.00139002 0.999066 -0.0431849 -0.0553842
  5988. -0.000416312 0.0431856 0.999067 -0.0142108
  5989. 0 0 0 1
  5990. shift: 0.0587415
  5991. -----------------------------------------------------------------
  5992. (Processed/Input): (315 / 315)
  5993. -----------------------------------------------------------------
  5994. Sequence number: 5245
  5995. Number of scan points: 6915 points.
  5996. Number of filtered scan points: 1153 points.
  5997. transformed_scan_ptr: 6915 points.
  5998. map: 6294 points.
  5999. NDT has converged: 1
  6000. Fitness score: 27.2967
  6001. Number of iteration: 5
  6002. (x,y,z,roll,pitch,yaw):
  6003. (-0.00321482, -0.163607, -0.0169831, 0.05557, -0.00148258, -0.00444239)
  6004. Transformation Matrix:
  6005. 0.999989 0.00435318 -0.00172701 -0.00321482
  6006. -0.00444237 0.998447 -0.0555343 -0.163607
  6007. 0.00148258 0.0555414 0.998455 -0.0169831
  6008. 0 0 0 1
  6009. shift: 0.163639
  6010. -----------------------------------------------------------------
  6011. (Processed/Input): (316 / 316)
  6012. -----------------------------------------------------------------
  6013. Sequence number: 5246
  6014. Number of scan points: 6907 points.
  6015. Number of filtered scan points: 1141 points.
  6016. transformed_scan_ptr: 6907 points.
  6017. map: 6294 points.
  6018. NDT has converged: 1
  6019. Fitness score: 28.743
  6020. Number of iteration: 3
  6021. (x,y,z,roll,pitch,yaw):
  6022. (0.0107295, -0.195299, -0.0207221, 0.0635129, 0.000946938, -0.0032093)
  6023. Transformation Matrix:
  6024. 0.999994 0.00326293 0.000741328 0.0107295
  6025. -0.0032093 0.997978 -0.0634729 -0.195299
  6026. -0.000946938 0.0634701 0.997983 -0.0207221
  6027. 0 0 0 1
  6028. shift: 0.195593
  6029. -----------------------------------------------------------------
  6030. (Processed/Input): (317 / 317)
  6031. -----------------------------------------------------------------
  6032. Sequence number: 5247
  6033. Number of scan points: 6918 points.
  6034. Number of filtered scan points: 1149 points.
  6035. transformed_scan_ptr: 6918 points.
  6036. map: 6294 points.
  6037. NDT has converged: 1
  6038. Fitness score: 27.341
  6039. Number of iteration: 3
  6040. (x,y,z,roll,pitch,yaw):
  6041. (0.0106173, -0.226631, -0.0206895, 0.0704721, 0.00463346, -0.00549211)
  6042. Transformation Matrix:
  6043. 0.999974 0.00580471 0.00423515 0.0106173
  6044. -0.00549203 0.997501 -0.0704381 -0.226631
  6045. -0.00463344 0.0704131 0.997507 -0.0206895
  6046. 0 0 0 1
  6047. shift: 0.22688
  6048. -----------------------------------------------------------------
  6049. (Processed/Input): (318 / 318)
  6050. -----------------------------------------------------------------
  6051. Sequence number: 5248
  6052. Number of scan points: 6938 points.
  6053. Number of filtered scan points: 1173 points.
  6054. transformed_scan_ptr: 6938 points.
  6055. map: 6294 points.
  6056. NDT has converged: 1
  6057. Fitness score: 27.8619
  6058. Number of iteration: 3
  6059. (x,y,z,roll,pitch,yaw):
  6060. (0.0534253, -0.246985, -0.0237534, 0.0785953, 0.0110533, -0.00605346)
  6061. Transformation Matrix:
  6062. 0.999921 0.00690254 0.0105435 0.0534253
  6063. -0.00605305 0.996889 -0.0785796 -0.246985
  6064. -0.0110531 0.0785096 0.996852 -0.0237534
  6065. 0 0 0 1
  6066. shift: 0.252697
  6067. -----------------------------------------------------------------
  6068. (Processed/Input): (319 / 319)
  6069. -----------------------------------------------------------------
  6070. Sequence number: 5249
  6071. Number of scan points: 6922 points.
  6072. Number of filtered scan points: 1173 points.
  6073. transformed_scan_ptr: 6922 points.
  6074. map: 6294 points.
  6075. NDT has converged: 1
  6076. Fitness score: 27.6064
  6077. Number of iteration: 3
  6078. (x,y,z,roll,pitch,yaw):
  6079. (0.0696521, -0.252689, -0.0272174, 0.0851114, 0.015047, 0.00169889)
  6080. Transformation Matrix:
  6081. 0.999885 -0.000413667 0.0151363 0.0696521
  6082. 0.0016987 0.996381 -0.0849831 -0.252689
  6083. -0.0150464 0.0849991 0.996267 -0.0272174
  6084. 0 0 0 1
  6085. shift: 0.262113
  6086. -----------------------------------------------------------------
  6087. (Processed/Input): (320 / 320)
  6088. -----------------------------------------------------------------
  6089. Sequence number: 5250
  6090. Number of scan points: 6977 points.
  6091. Number of filtered scan points: 1201 points.
  6092. transformed_scan_ptr: 6977 points.
  6093. map: 6294 points.
  6094. NDT has converged: 1
  6095. Fitness score: 28.2178
  6096. Number of iteration: 3
  6097. (x,y,z,roll,pitch,yaw):
  6098. (0.0809414, -0.249778, -0.0377089, 0.0893477, 0.016157, 3.9566e-05)
  6099. Transformation Matrix:
  6100. 0.999869 0.00140221 0.0160954 0.0809414
  6101. 3.95608e-05 0.996011 -0.0892282 -0.249778
  6102. -0.0161563 0.0892172 0.995881 -0.0377089
  6103. 0 0 0 1
  6104. shift: 0.262566
  6105. -----------------------------------------------------------------
  6106. (Processed/Input): (321 / 321)
  6107. -----------------------------------------------------------------
  6108. Sequence number: 5251
  6109. Number of scan points: 7029 points.
  6110. Number of filtered scan points: 1199 points.
  6111. transformed_scan_ptr: 7029 points.
  6112. map: 6294 points.
  6113. NDT has converged: 1
  6114. Fitness score: 27.7145
  6115. Number of iteration: 2
  6116. (x,y,z,roll,pitch,yaw):
  6117. (0.0904867, -0.260383, -0.0427997, 0.088714, 0.0143395, 0.00760369)
  6118. Transformation Matrix:
  6119. 0.999868 -0.00630335 0.0149559 0.0904867
  6120. 0.00760284 0.996048 -0.0884865 -0.260383
  6121. -0.014339 0.0885885 0.995965 -0.0427997
  6122. 0 0 0 1
  6123. shift: 0.275657
  6124. -----------------------------------------------------------------
  6125. (Processed/Input): (322 / 322)
  6126. -----------------------------------------------------------------
  6127. Sequence number: 5252
  6128. Number of scan points: 7174 points.
  6129. Number of filtered scan points: 1192 points.
  6130. transformed_scan_ptr: 7174 points.
  6131. map: 6294 points.
  6132. NDT has converged: 1
  6133. Fitness score: 27.8294
  6134. Number of iteration: 6
  6135. (x,y,z,roll,pitch,yaw):
  6136. (0.00160655, -0.2122, -0.0501565, 0.08133, 0.0115309, -0.0120349)
  6137. Transformation Matrix:
  6138. 0.999861 0.0129315 0.010514 0.00160655
  6139. -0.0120338 0.996611 -0.0813728 -0.2122
  6140. -0.0115307 0.0812349 0.996628 -0.0501565
  6141. 0 0 0 1
  6142. shift: 0.212206
  6143. -----------------------------------------------------------------
  6144. (Processed/Input): (323 / 323)
  6145. -----------------------------------------------------------------
  6146. Sequence number: 5253
  6147. Number of scan points: 7023 points.
  6148. Number of filtered scan points: 1215 points.
  6149. transformed_scan_ptr: 7023 points.
  6150. map: 6294 points.
  6151. NDT has converged: 1
  6152. Fitness score: 28.4224
  6153. Number of iteration: 5
  6154. (x,y,z,roll,pitch,yaw):
  6155. (0.0166025, -0.264128, -0.0584933, 0.0767746, 0.00876031, -0.0154112)
  6156. Transformation Matrix:
  6157. 0.999843 0.016037 0.00755138 0.0166025
  6158. -0.01541 0.996926 -0.0768247 -0.264128
  6159. -0.0087602 0.0766962 0.997016 -0.0584933
  6160. 0 0 0 1
  6161. shift: 0.264649
  6162. -----------------------------------------------------------------
  6163. (Processed/Input): (324 / 324)
  6164. -----------------------------------------------------------------
  6165. Sequence number: 5254
  6166. Number of scan points: 7072 points.
  6167. Number of filtered scan points: 1217 points.
  6168. transformed_scan_ptr: 7072 points.
  6169. map: 6294 points.
  6170. NDT has converged: 1
  6171. Fitness score: 28.4366
  6172. Number of iteration: 6
  6173. (x,y,z,roll,pitch,yaw):
  6174. (-0.0262126, -0.186815, -0.0620907, 0.0718902, 0.00654149, -0.00951522)
  6175. Transformation Matrix:
  6176. 0.999933 0.00996034 0.00584079 -0.0262126
  6177. -0.00951487 0.997367 -0.0718871 -0.186815
  6178. -0.00654144 0.0718267 0.997396 -0.0620907
  6179. 0 0 0 1
  6180. shift: 0.188645
  6181. -----------------------------------------------------------------
  6182. (Processed/Input): (325 / 325)
  6183. -----------------------------------------------------------------
  6184. Sequence number: 5255
  6185. Number of scan points: 7055 points.
  6186. Number of filtered scan points: 1217 points.
  6187. transformed_scan_ptr: 7055 points.
  6188. map: 6294 points.
  6189. NDT has converged: 1
  6190. Fitness score: 28.2149
  6191. Number of iteration: 4
  6192. (x,y,z,roll,pitch,yaw):
  6193. (-0.0146186, -0.204427, -0.065982, 0.0639764, 0.00405791, -0.00826552)
  6194. Transformation Matrix:
  6195. 0.999958 0.00850794 0.00352103 -0.0146186
  6196. -0.00826536 0.997918 -0.063964 -0.204427
  6197. -0.0040579 0.0639322 0.997946 -0.065982
  6198. 0 0 0 1
  6199. shift: 0.204949
  6200. -----------------------------------------------------------------
  6201. (Processed/Input): (326 / 326)
  6202. -----------------------------------------------------------------
  6203. Sequence number: 5256
  6204. Number of scan points: 7063 points.
  6205. Number of filtered scan points: 1196 points.
  6206. transformed_scan_ptr: 7063 points.
  6207. map: 6294 points.
  6208. NDT has converged: 1
  6209. Fitness score: 28.4614
  6210. Number of iteration: 4
  6211. (x,y,z,roll,pitch,yaw):
  6212. (0.0202733, -0.136769, -0.0695265, 0.0542928, 0.00335508, -0.00195896)
  6213. Transformation Matrix:
  6214. 0.999992 0.00213814 0.00324382 0.0202733
  6215. -0.00195895 0.998524 -0.0542725 -0.136769
  6216. -0.00335508 0.0542658 0.998521 -0.0695265
  6217. 0 0 0 1
  6218. shift: 0.138263
  6219. -----------------------------------------------------------------
  6220. (Processed/Input): (327 / 327)
  6221. -----------------------------------------------------------------
  6222. Sequence number: 5257
  6223. Number of scan points: 7139 points.
  6224. Number of filtered scan points: 1221 points.
  6225. transformed_scan_ptr: 7139 points.
  6226. map: 6294 points.
  6227. NDT has converged: 1
  6228. Fitness score: 28.9484
  6229. Number of iteration: 3
  6230. (x,y,z,roll,pitch,yaw):
  6231. (0.0476328, -0.112583, -0.0714043, 0.0437346, 0.00135447, -0.00533234)
  6232. Transformation Matrix:
  6233. 0.999985 0.00538644 0.00112003 0.0476328
  6234. -0.00533231 0.999029 -0.0437273 -0.112583
  6235. -0.00135447 0.0437206 0.999043 -0.0714043
  6236. 0 0 0 1
  6237. shift: 0.122245
  6238. -----------------------------------------------------------------
  6239. (Processed/Input): (328 / 328)
  6240. -----------------------------------------------------------------
  6241. Sequence number: 5258
  6242. Number of scan points: 7089 points.
  6243. Number of filtered scan points: 1194 points.
  6244. transformed_scan_ptr: 7089 points.
  6245. map: 6294 points.
  6246. NDT has converged: 1
  6247. Fitness score: 28.3439
  6248. Number of iteration: 2
  6249. (x,y,z,roll,pitch,yaw):
  6250. (0.0365596, -0.0933771, -0.0751192, 0.0366116, -0.000171688, -0.00359093)
  6251. Transformation Matrix:
  6252. 0.999994 0.00358223 -0.00030301 0.0365596
  6253. -0.00359092 0.999323 -0.0366026 -0.0933771
  6254. 0.000171688 0.0366034 0.99933 -0.0751192
  6255. 0 0 0 1
  6256. shift: 0.100279
  6257. -----------------------------------------------------------------
  6258. (Processed/Input): (329 / 329)
  6259. -----------------------------------------------------------------
  6260. Sequence number: 5259
  6261. Number of scan points: 7078 points.
  6262. Number of filtered scan points: 1188 points.
  6263. transformed_scan_ptr: 7078 points.
  6264. map: 6294 points.
  6265. NDT has converged: 1
  6266. Fitness score: 28.518
  6267. Number of iteration: 3
  6268. (x,y,z,roll,pitch,yaw):
  6269. (0.0440829, -0.110874, -0.068697, 0.0241438, -0.000256876, -0.000494746)
  6270. Transformation Matrix:
  6271. 1 0.000488401 -0.000268744 0.0440829
  6272. -0.000494746 0.999708 -0.0241413 -0.110874
  6273. 0.000256876 0.0241414 0.999709 -0.068697
  6274. 0 0 0 1
  6275. shift: 0.119317
  6276. -----------------------------------------------------------------
  6277. (Processed/Input): (330 / 330)
  6278. -----------------------------------------------------------------
  6279. Sequence number: 5260
  6280. Number of scan points: 7138 points.
  6281. Number of filtered scan points: 1186 points.
  6282. transformed_scan_ptr: 7138 points.
  6283. map: 6294 points.
  6284. NDT has converged: 1
  6285. Fitness score: 29.5057
  6286. Number of iteration: 4
  6287. (x,y,z,roll,pitch,yaw):
  6288. (0.0194919, -0.0418789, -0.0673187, 0.0117634, -0.00337667, -0.0037807)
  6289. Transformation Matrix:
  6290. 0.999987 0.00374071 -0.00342087 0.0194919
  6291. -0.00378067 0.999924 -0.0117503 -0.0418789
  6292. 0.00337666 0.0117631 0.999925 -0.0673187
  6293. 0 0 0 1
  6294. shift: 0.0461928
  6295. -----------------------------------------------------------------
  6296. (Processed/Input): (331 / 331)
  6297. -----------------------------------------------------------------
  6298. Sequence number: 5261
  6299. Number of scan points: 7081 points.
  6300. Number of filtered scan points: 1210 points.
  6301. transformed_scan_ptr: 7081 points.
  6302. map: 6294 points.
  6303. NDT has converged: 1
  6304. Fitness score: 28.9306
  6305. Number of iteration: 2
  6306. (x,y,z,roll,pitch,yaw):
  6307. (0.00950779, -0.0297796, -0.0614041, 0.000666504, -0.00432721, -0.00281897)
  6308. Transformation Matrix:
  6309. 0.999987 0.00281609 -0.00432906 0.00950779
  6310. -0.00281894 0.999996 -0.000654324 -0.0297796
  6311. 0.0043272 0.000666498 0.99999 -0.0614041
  6312. 0 0 0 1
  6313. shift: 0.0312606
  6314. -----------------------------------------------------------------
  6315. (Processed/Input): (332 / 332)
  6316. -----------------------------------------------------------------
  6317. Sequence number: 5262
  6318. Number of scan points: 7073 points.
  6319. Number of filtered scan points: 1226 points.
  6320. transformed_scan_ptr: 7073 points.
  6321. map: 6294 points.
  6322. NDT has converged: 1
  6323. Fitness score: 29.1734
  6324. Number of iteration: 2
  6325. (x,y,z,roll,pitch,yaw):
  6326. (0.00258701, -0.0210702, -0.0582593, -0.00906405, -0.00449304, -0.000727929)
  6327. Transformation Matrix:
  6328. 0.99999 0.000768613 -0.00448624 0.00258701
  6329. -0.000727922 0.999959 0.0090672 -0.0210702
  6330. 0.00449302 -0.00906384 0.999949 -0.0582593
  6331. 0 0 0 1
  6332. shift: 0.0212284
  6333. -----------------------------------------------------------------
  6334. (Processed/Input): (333 / 333)
  6335. -----------------------------------------------------------------
  6336. Sequence number: 5263
  6337. Number of scan points: 7116 points.
  6338. Number of filtered scan points: 1233 points.
  6339. transformed_scan_ptr: 7116 points.
  6340. map: 6294 points.
  6341. NDT has converged: 1
  6342. Fitness score: 29.4898
  6343. Number of iteration: 2
  6344. (x,y,z,roll,pitch,yaw):
  6345. (-0.0108622, 0.0549578, -0.0535503, -0.0232746, -0.0040679, -0.00170783)
  6346. Transformation Matrix:
  6347. 0.99999 0.00180204 -0.00402703 -0.0108622
  6348. -0.00170781 0.999727 0.0232794 0.0549578
  6349. 0.00406788 -0.0232723 0.999721 -0.0535503
  6350. 0 0 0 1
  6351. shift: 0.0560209
  6352. -----------------------------------------------------------------
  6353. (Processed/Input): (334 / 334)
  6354. -----------------------------------------------------------------
  6355. Sequence number: 5264
  6356. Number of scan points: 7090 points.
  6357. Number of filtered scan points: 1238 points.
  6358. transformed_scan_ptr: 7090 points.
  6359. map: 6294 points.
  6360. NDT has converged: 1
  6361. Fitness score: 29.1743
  6362. Number of iteration: 3
  6363. (x,y,z,roll,pitch,yaw):
  6364. (-0.0321878, 0.0571326, -0.053051, -0.0361132, -0.00349434, 0.00169638)
  6365. Transformation Matrix:
  6366. 0.999992 -0.00156911 -0.0035533 -0.0321878
  6367. 0.00169637 0.999347 0.0360994 0.0571326
  6368. 0.00349433 -0.0361051 0.999342 -0.053051
  6369. 0 0 0 1
  6370. shift: 0.0655759
  6371. -----------------------------------------------------------------
  6372. (Processed/Input): (335 / 335)
  6373. -----------------------------------------------------------------
  6374. Sequence number: 5265
  6375. Number of scan points: 7061 points.
  6376. Number of filtered scan points: 1223 points.
  6377. transformed_scan_ptr: 7061 points.
  6378. map: 6294 points.
  6379. NDT has converged: 1
  6380. Fitness score: 29.0041
  6381. Number of iteration: 3
  6382. (x,y,z,roll,pitch,yaw):
  6383. (0.0103182, 0.0714623, -0.0468273, -0.0436874, -0.00159908, 0.000873576)
  6384. Transformation Matrix:
  6385. 0.999998 -0.000802906 -0.0016357 0.0103182
  6386. 0.000873574 0.999046 0.0436721 0.0714623
  6387. 0.00159908 -0.0436735 0.999045 -0.0468273
  6388. 0 0 0 1
  6389. shift: 0.0722034
  6390. -----------------------------------------------------------------
  6391. (Processed/Input): (336 / 336)
  6392. -----------------------------------------------------------------
  6393. Sequence number: 5266
  6394. Number of scan points: 7212 points.
  6395. Number of filtered scan points: 1264 points.
  6396. transformed_scan_ptr: 7212 points.
  6397. map: 6294 points.
  6398. NDT has converged: 1
  6399. Fitness score: 30.3298
  6400. Number of iteration: 3
  6401. (x,y,z,roll,pitch,yaw):
  6402. (-0.00818407, 0.0803421, -0.0421482, -0.0522927, 0.000264805, 0.00295478)
  6403. Transformation Matrix:
  6404. 0.999996 -0.00296458 0.000109997 -0.00818407
  6405. 0.00295478 0.998629 0.0522694 0.0803421
  6406. -0.000264805 -0.0522688 0.998633 -0.0421482
  6407. 0 0 0 1
  6408. shift: 0.0807579
  6409. -----------------------------------------------------------------
  6410. (Processed/Input): (337 / 337)
  6411. -----------------------------------------------------------------
  6412. Sequence number: 5267
  6413. Number of scan points: 7110 points.
  6414. Number of filtered scan points: 1298 points.
  6415. transformed_scan_ptr: 7110 points.
  6416. map: 6294 points.
  6417. NDT has converged: 1
  6418. Fitness score: 29.6158
  6419. Number of iteration: 4
  6420. (x,y,z,roll,pitch,yaw):
  6421. (0.0390188, 0.0603136, -0.0404573, -0.0570108, 0.000137804, -0.00303602)
  6422. Transformation Matrix:
  6423. 0.999995 0.00302323 0.000310573 0.0390188
  6424. -0.00303601 0.998371 0.0569792 0.0603136
  6425. -0.000137804 -0.0569799 0.998375 -0.0404573
  6426. 0 0 0 1
  6427. shift: 0.0718345
  6428. -----------------------------------------------------------------
  6429. (Processed/Input): (338 / 338)
  6430. -----------------------------------------------------------------
  6431. Sequence number: 5268
  6432. Number of scan points: 7257 points.
  6433. Number of filtered scan points: 1295 points.
  6434. transformed_scan_ptr: 7257 points.
  6435. map: 6294 points.
  6436. NDT has converged: 1
  6437. Fitness score: 30.5608
  6438. Number of iteration: 3
  6439. (x,y,z,roll,pitch,yaw):
  6440. (0.0288527, 0.0642658, -0.0383967, -0.0577477, -0.000729017, -0.00269437)
  6441. Transformation Matrix:
  6442. 0.999996 0.00273195 -0.000572291 0.0288527
  6443. -0.00269436 0.998329 0.0577173 0.0642658
  6444. 0.000729017 -0.0577156 0.998333 -0.0383967
  6445. 0 0 0 1
  6446. shift: 0.0704456
  6447. -----------------------------------------------------------------
  6448. (Processed/Input): (339 / 339)
  6449. -----------------------------------------------------------------
  6450. Sequence number: 5269
  6451. Number of scan points: 7087 points.
  6452. Number of filtered scan points: 1293 points.
  6453. transformed_scan_ptr: 7087 points.
  6454. map: 6294 points.
  6455. NDT has converged: 1
  6456. Fitness score: 29.6664
  6457. Number of iteration: 2
  6458. (x,y,z,roll,pitch,yaw):
  6459. (0.0207932, 0.0860492, -0.0352017, -0.0560194, -0.00233114, 0.00202468)
  6460. Transformation Matrix:
  6461. 0.999995 -0.00189098 -0.00244084 0.0207932
  6462. 0.00202467 0.99843 0.0559852 0.0860492
  6463. 0.00233114 -0.0559899 0.998429 -0.0352017
  6464. 0 0 0 1
  6465. shift: 0.0885258
  6466. -----------------------------------------------------------------
  6467. (Processed/Input): (340 / 340)
  6468. (Processed/Input): (341 / 341)
  6469. -----------------------------------------------------------------
  6470. Sequence number: 5270
  6471. Number of scan points: 7137 points.
  6472. Number of filtered scan points: 1270 points.
  6473. transformed_scan_ptr: 7137 points.
  6474. map: 6294 points.
  6475. NDT has converged: 1
  6476. Fitness score: 29.8945
  6477. Number of iteration: 2
  6478. (x,y,z,roll,pitch,yaw):
  6479. (0.0222051, 0.111477, -0.0335059, -0.0523631, -0.0034755, -0.00214402)
  6480. Transformation Matrix:
  6481. 0.999992 0.00232298 -0.00335851 0.0222051
  6482. -0.00214401 0.998627 0.0523465 0.111477
  6483. 0.0034755 -0.0523388 0.998623 -0.0335059
  6484. 0 0 0 1
  6485. shift: 0.113667
  6486. -----------------------------------------------------------------
  6487. -----------------------------------------------------------------
  6488. Sequence number: 5271
  6489. Number of scan points: 7172 points.
  6490. Number of filtered scan points: 1288 points.
  6491. transformed_scan_ptr: 7172 points.
  6492. map: 6294 points.
  6493. NDT has converged: 1
  6494. Fitness score: 30.5847
  6495. Number of iteration: 2
  6496. (x,y,z,roll,pitch,yaw):
  6497. (0.0276509, 0.0661089, -0.0346806, -0.0466114, -0.00304792, 0.00273572)
  6498. Transformation Matrix:
  6499. 0.999992 -0.00259073 -0.00317207 0.0276509
  6500. 0.0027357 0.998911 0.046586 0.0661089
  6501. 0.00304792 -0.0465943 0.998909 -0.0346806
  6502. 0 0 0 1
  6503. shift: 0.0716586
  6504. -----------------------------------------------------------------
  6505. (Processed/Input): (342 / 342)
  6506. -----------------------------------------------------------------
  6507. Sequence number: 5272
  6508. Number of scan points: 7222 points.
  6509. Number of filtered scan points: 1268 points.
  6510. transformed_scan_ptr: 7222 points.
  6511. map: 6294 points.
  6512. NDT has converged: 1
  6513. Fitness score: 30.644
  6514. Number of iteration: 2
  6515. (x,y,z,roll,pitch,yaw):
  6516. (0.0215152, 0.0544073, -0.0363872, -0.0361759, -0.00127469, -0.00369981)
  6517. Transformation Matrix:
  6518. 0.999992 0.00374349 -0.00114003 0.0215152
  6519. -0.0036998 0.999339 0.0361725 0.0544073
  6520. 0.00127469 -0.036168 0.999345 -0.0363872
  6521. 0 0 0 1
  6522. shift: 0.0585069
  6523. -----------------------------------------------------------------
  6524. (Processed/Input): (343 / 343)
  6525. -----------------------------------------------------------------
  6526. Sequence number: 5273
  6527. Number of scan points: 7247 points.
  6528. Number of filtered scan points: 1260 points.
  6529. transformed_scan_ptr: 7247 points.
  6530. map: 6294 points.
  6531. NDT has converged: 1
  6532. Fitness score: 29.9101
  6533. Number of iteration: 2
  6534. (x,y,z,roll,pitch,yaw):
  6535. (0.01744, 0.0213584, -0.0464163, -0.0265441, 0.00191915, -0.00547775)
  6536. Transformation Matrix:
  6537. 0.999983 0.00542485 0.00206384 0.01744
  6538. -0.00547771 0.999633 0.0265301 0.0213584
  6539. -0.00191915 -0.026541 0.999646 -0.0464163
  6540. 0 0 0 1
  6541. shift: 0.0275742
  6542. -----------------------------------------------------------------
  6543. (Processed/Input): (344 / 344)
  6544. -----------------------------------------------------------------
  6545. Sequence number: 5274
  6546. Number of scan points: 7289 points.
  6547. Number of filtered scan points: 1264 points.
  6548. transformed_scan_ptr: 7289 points.
  6549. map: 6294 points.
  6550. NDT has converged: 1
  6551. Fitness score: 29.9506
  6552. Number of iteration: 3
  6553. (x,y,z,roll,pitch,yaw):
  6554. (0.0112714, -0.0230841, -0.0627817, -0.00725053, 0.00593116, -0.00412419)
  6555. Transformation Matrix:
  6556. 0.999974 0.00408108 0.00596081 0.0112714
  6557. -0.00412411 0.999965 0.00722595 -0.0230841
  6558. -0.00593113 -0.00725034 0.999956 -0.0627817
  6559. 0 0 0 1
  6560. shift: 0.0256889
  6561. -----------------------------------------------------------------
  6562. (Processed/Input): (345 / 345)
  6563. -----------------------------------------------------------------
  6564. Sequence number: 5275
  6565. Number of scan points: 7310 points.
  6566. Number of filtered scan points: 1267 points.
  6567. transformed_scan_ptr: 7310 points.
  6568. map: 6294 points.
  6569. NDT has converged: 1
  6570. Fitness score: 30.2682
  6571. Number of iteration: 4
  6572. (x,y,z,roll,pitch,yaw):
  6573. (0.0597182, -0.0376126, -0.0872623, 0.00796952, 0.00889344, 0.000898762)
  6574. Transformation Matrix:
  6575. 0.99996 -0.000827829 0.0089002 0.0597182
  6576. 0.000898727 0.999968 -0.00796144 -0.0376126
  6577. -0.00889332 0.00796912 0.999929 -0.0872623
  6578. 0 0 0 1
  6579. shift: 0.070576
  6580. -----------------------------------------------------------------
  6581. (Processed/Input): (346 / 346)
  6582. -----------------------------------------------------------------
  6583. Sequence number: 5276
  6584. Number of scan points: 7347 points.
  6585. Number of filtered scan points: 1271 points.
  6586. transformed_scan_ptr: 7347 points.
  6587. map: 6294 points.
  6588. NDT has converged: 1
  6589. Fitness score: 30.3087
  6590. Number of iteration: 3
  6591. (x,y,z,roll,pitch,yaw):
  6592. (0.098143, -0.0699493, -0.105713, 0.0239188, 0.00746288, 0.00254893)
  6593. Transformation Matrix:
  6594. 0.999969 -0.0023697 0.00752162 0.098143
  6595. 0.00254885 0.999711 -0.0238974 -0.0699493
  6596. -0.00746281 0.0239158 0.999686 -0.105713
  6597. 0 0 0 1
  6598. shift: 0.12052
  6599. -----------------------------------------------------------------
  6600. (Processed/Input): (347 / 347)
  6601. -----------------------------------------------------------------
  6602. Sequence number: 5277
  6603. Number of scan points: 7161 points.
  6604. Number of filtered scan points: 1238 points.
  6605. transformed_scan_ptr: 7161 points.
  6606. map: 6294 points.
  6607. NDT has converged: 1
  6608. Fitness score: 30.0232
  6609. Number of iteration: 6
  6610. (x,y,z,roll,pitch,yaw):
  6611. (0.0682065, -0.128604, -0.112432, 0.0368185, 0.00535479, -0.00240327)
  6612. Transformation Matrix:
  6613. 0.999983 0.00259875 0.00526266 0.0682065
  6614. -0.00240323 0.999319 -0.0368229 -0.128604
  6615. -0.00535477 0.0368096 0.999308 -0.112432
  6616. 0 0 0 1
  6617. shift: 0.145571
  6618. -----------------------------------------------------------------
  6619. (Processed/Input): (348 / 348)
  6620. -----------------------------------------------------------------
  6621. Sequence number: 5278
  6622. Number of scan points: 7299 points.
  6623. Number of filtered scan points: 1230 points.
  6624. transformed_scan_ptr: 7299 points.
  6625. map: 6294 points.
  6626. NDT has converged: 1
  6627. Fitness score: 29.6912
  6628. Number of iteration: 2
  6629. (x,y,z,roll,pitch,yaw):
  6630. (0.00541369, -0.18644, -0.114849, 0.0487166, 0.000879839, -0.0073945)
  6631. Transformation Matrix:
  6632. 0.999972 0.00742851 0.000518689 0.00541369
  6633. -0.00739443 0.998786 -0.0487025 -0.18644
  6634. -0.000879839 0.0486973 0.998813 -0.114849
  6635. 0 0 0 1
  6636. shift: 0.186518
  6637. -----------------------------------------------------------------
  6638. (Processed/Input): (349 / 349)
  6639. -----------------------------------------------------------------
  6640. Sequence number: 5279
  6641. Number of scan points: 7331 points.
  6642. Number of filtered scan points: 1239 points.
  6643. transformed_scan_ptr: 7331 points.
  6644. map: 6294 points.
  6645. NDT has converged: 1
  6646. Fitness score: 29.6311
  6647. Number of iteration: 8
  6648. (x,y,z,roll,pitch,yaw):
  6649. (-0.146362, -0.192223, -0.113273, 0.0529187, -0.00419416, -0.0277416)
  6650. Transformation Matrix:
  6651. 0.999606 0.0274774 -0.00565386 -0.146362
  6652. -0.0277378 0.998222 -0.0527574 -0.192223
  6653. 0.00419415 0.0528935 0.998591 -0.113273
  6654. 0 0 0 1
  6655. shift: 0.241602
  6656. -----------------------------------------------------------------
  6657. (Processed/Input): (350 / 350)
  6658. -----------------------------------------------------------------
  6659. Sequence number: 5280
  6660. Number of scan points: 7272 points.
  6661. Number of filtered scan points: 1255 points.
  6662. transformed_scan_ptr: 7272 points.
  6663. map: 6294 points.
  6664. NDT has converged: 1
  6665. Fitness score: 29.6257
  6666. Number of iteration: 8
  6667. (x,y,z,roll,pitch,yaw):
  6668. (0.0163692, -0.156384, -0.115459, 0.0532346, -0.0045453, -0.0103614)
  6669. Transformation Matrix:
  6670. 0.999936 0.0101047 -0.00508991 0.0163692
  6671. -0.0103611 0.998532 -0.0531596 -0.156384
  6672. 0.00454529 0.0532089 0.998573 -0.115459
  6673. 0 0 0 1
  6674. shift: 0.157239
  6675. -----------------------------------------------------------------
  6676. (Processed/Input): (351 / 351)
  6677. -----------------------------------------------------------------
  6678. Sequence number: 5281
  6679. Number of scan points: 7281 points.
  6680. Number of filtered scan points: 1219 points.
  6681. transformed_scan_ptr: 7281 points.
  6682. map: 6294 points.
  6683. NDT has converged: 1
  6684. Fitness score: 29.4734
  6685. Number of iteration: 7
  6686. (x,y,z,roll,pitch,yaw):
  6687. (-0.0257131, -0.146898, -0.125111, 0.051138, -0.00540706, -0.0127445)
  6688. Transformation Matrix:
  6689. 0.999904 0.0124512 -0.00605094 -0.0257131
  6690. -0.012744 0.998615 -0.0510427 -0.146898
  6691. 0.00540703 0.0511149 0.998678 -0.125111
  6692. 0 0 0 1
  6693. shift: 0.149131
  6694. -----------------------------------------------------------------
  6695. (Processed/Input): (352 / 352)
  6696. -----------------------------------------------------------------
  6697. Sequence number: 5282
  6698. Number of scan points: 7122 points.
  6699. Number of filtered scan points: 1227 points.
  6700. transformed_scan_ptr: 7122 points.
  6701. map: 6294 points.
  6702. NDT has converged: 1
  6703. Fitness score: 29.5728
  6704. Number of iteration: 4
  6705. (x,y,z,roll,pitch,yaw):
  6706. (-0.00496222, -0.165197, -0.124935, 0.0468316, -0.00502806, -0.0066438)
  6707. Transformation Matrix:
  6708. 0.999965 0.00640109 -0.00533343 -0.00496222
  6709. -0.00664367 0.998883 -0.0467801 -0.165197
  6710. 0.00502804 0.0468139 0.998891 -0.124935
  6711. 0 0 0 1
  6712. shift: 0.165272
  6713. -----------------------------------------------------------------
  6714. (Processed/Input): (353 / 353)
  6715. -----------------------------------------------------------------
  6716. Sequence number: 5283
  6717. Number of scan points: 7181 points.
  6718. Number of filtered scan points: 1242 points.
  6719. transformed_scan_ptr: 7181 points.
  6720. map: 6294 points.
  6721. NDT has converged: 1
  6722. Fitness score: 29.1712
  6723. Number of iteration: 3
  6724. (x,y,z,roll,pitch,yaw):
  6725. (0.0109563, -0.149546, -0.118116, 0.04024, -0.00503696, -0.00345195)
  6726. Transformation Matrix:
  6727. 0.999981 0.00324652 -0.0051717 0.0109563
  6728. -0.0034519 0.999185 -0.0402115 -0.149546
  6729. 0.00503694 0.0402286 0.999178 -0.118116
  6730. 0 0 0 1
  6731. shift: 0.149947
  6732. -----------------------------------------------------------------
  6733. (Processed/Input): (354 / 354)
  6734. -----------------------------------------------------------------
  6735. Sequence number: 5284
  6736. Number of scan points: 7324 points.
  6737. Number of filtered scan points: 1255 points.
  6738. transformed_scan_ptr: 7324 points.
  6739. map: 6294 points.
  6740. NDT has converged: 1
  6741. Fitness score: 29.3514
  6742. Number of iteration: 3
  6743. (x,y,z,roll,pitch,yaw):
  6744. (-0.0187185, -0.115977, -0.118303, 0.0369926, -0.00504401, -0.0151624)
  6745. Transformation Matrix:
  6746. 0.999872 0.0149649 -0.00560072 -0.0187185
  6747. -0.0151616 0.999204 -0.0369035 -0.115977
  6748. 0.00504399 0.0369837 0.999303 -0.118303
  6749. 0 0 0 1
  6750. shift: 0.117478
  6751. -----------------------------------------------------------------
  6752. (Processed/Input): (355 / 355)
  6753. -----------------------------------------------------------------
  6754. Sequence number: 5285
  6755. Number of scan points: 7296 points.
  6756. Number of filtered scan points: 1250 points.
  6757. transformed_scan_ptr: 7296 points.
  6758. map: 6294 points.
  6759. NDT has converged: 1
  6760. Fitness score: 29.2911
  6761. Number of iteration: 3
  6762. (x,y,z,roll,pitch,yaw):
  6763. (-0.0724457, -0.128936, -0.124903, 0.0376617, -0.00466648, -0.0210821)
  6764. Transformation Matrix:
  6765. 0.999767 0.0208899 -0.00545584 -0.0724457
  6766. -0.0210803 0.999073 -0.0375461 -0.128936
  6767. 0.00466646 0.0376524 0.99928 -0.124903
  6768. 0 0 0 1
  6769. shift: 0.147895
  6770. -----------------------------------------------------------------
  6771. (Processed/Input): (356 / 356)
  6772. -----------------------------------------------------------------
  6773. Sequence number: 5286
  6774. Number of scan points: 7375 points.
  6775. Number of filtered scan points: 1259 points.
  6776. transformed_scan_ptr: 7375 points.
  6777. map: 6294 points.
  6778. NDT has converged: 1
  6779. Fitness score: 29.5077
  6780. Number of iteration: 5
  6781. (x,y,z,roll,pitch,yaw):
  6782. (-0.054381, -0.112585, -0.125867, 0.0373023, -0.00485217, -0.00848241)
  6783. Transformation Matrix:
  6784. 0.999952 0.00829546 -0.00516494 -0.054381
  6785. -0.00848221 0.99927 -0.0372512 -0.112585
  6786. 0.00485215 0.0372932 0.999293 -0.125867
  6787. 0 0 0 1
  6788. shift: 0.125031
  6789. -----------------------------------------------------------------
  6790. (Processed/Input): (357 / 357)
  6791. -----------------------------------------------------------------
  6792. Sequence number: 5287
  6793. Number of scan points: 7280 points.
  6794. Number of filtered scan points: 1256 points.
  6795. transformed_scan_ptr: 7280 points.
  6796. map: 6294 points.
  6797. NDT has converged: 1
  6798. Fitness score: 28.325
  6799. Number of iteration: 3
  6800. (x,y,z,roll,pitch,yaw):
  6801. (0.0264445, -0.130791, -0.136004, 0.0408493, -0.00672493, -0.00406541)
  6802. Transformation Matrix:
  6803. 0.999969 0.00378738 -0.00688524 0.0264445
  6804. -0.0040653 0.999159 -0.0408103 -0.130791
  6805. 0.00672488 0.040837 0.999143 -0.136004
  6806. 0 0 0 1
  6807. shift: 0.133438
  6808. -----------------------------------------------------------------
  6809. (Processed/Input): (358 / 358)
  6810. -----------------------------------------------------------------
  6811. Sequence number: 5288
  6812. Number of scan points: 7429 points.
  6813. Number of filtered scan points: 1241 points.
  6814. transformed_scan_ptr: 7429 points.
  6815. map: 6294 points.
  6816. NDT has converged: 1
  6817. Fitness score: 28.0419
  6818. Number of iteration: 5
  6819. (x,y,z,roll,pitch,yaw):
  6820. (0.0096785, -0.143535, -0.130457, 0.0394229, -0.00696539, -0.00264637)
  6821. Transformation Matrix:
  6822. 0.999972 0.00236979 -0.0070642 0.0096785
  6823. -0.00264631 0.99922 -0.0393942 -0.143535
  6824. 0.00696534 0.0394118 0.999199 -0.130457
  6825. 0 0 0 1
  6826. shift: 0.143861
  6827. -----------------------------------------------------------------
  6828. (Processed/Input): (359 / 359)
  6829. -----------------------------------------------------------------
  6830. Sequence number: 5289
  6831. Number of scan points: 7387 points.
  6832. Number of filtered scan points: 1248 points.
  6833. transformed_scan_ptr: 7387 points.
  6834. map: 6294 points.
  6835. NDT has converged: 1
  6836. Fitness score: 27.5927
  6837. Number of iteration: 3
  6838. (x,y,z,roll,pitch,yaw):
  6839. (-0.0693996, -0.151227, -0.120141, 0.0370914, -0.00939525, -0.0114211)
  6840. Transformation Matrix:
  6841. 0.999891 0.0110646 -0.00981155 -0.0693996
  6842. -0.0114203 0.999251 -0.0369733 -0.151227
  6843. 0.00939511 0.0370813 0.999268 -0.120141
  6844. 0 0 0 1
  6845. shift: 0.16639
  6846. -----------------------------------------------------------------
  6847. (Processed/Input): (360 / 360)
  6848. -----------------------------------------------------------------
  6849. Sequence number: 5290
  6850. Number of scan points: 7558 points.
  6851. Number of filtered scan points: 1261 points.
  6852. transformed_scan_ptr: 7558 points.
  6853. map: 6294 points.
  6854. NDT has converged: 1
  6855. Fitness score: 27.3197
  6856. Number of iteration: 4
  6857. (x,y,z,roll,pitch,yaw):
  6858. (-0.0455285, -0.170343, -0.113808, 0.038161, -0.00786673, -0.0144433)
  6859. Transformation Matrix:
  6860. 0.999865 0.0141322 -0.00841111 -0.0455285
  6861. -0.0144423 0.999172 -0.0380342 -0.170343
  6862. 0.00786665 0.0381506 0.999241 -0.113808
  6863. 0 0 0 1
  6864. shift: 0.176323
  6865. -----------------------------------------------------------------
  6866. (Processed/Input): (361 / 361)
  6867. -----------------------------------------------------------------
  6868. Sequence number: 5291
  6869. Number of scan points: 7493 points.
  6870. Number of filtered scan points: 1289 points.
  6871. transformed_scan_ptr: 7493 points.
  6872. map: 6294 points.
  6873. NDT has converged: 1
  6874. Fitness score: 27.4
  6875. Number of iteration: 3
  6876. (x,y,z,roll,pitch,yaw):
  6877. (-0.00557117, -0.159048, -0.113326, 0.0401937, -0.0052193, -0.0105179)
  6878. Transformation Matrix:
  6879. 0.999931 0.0102995 -0.00563741 -0.00557117
  6880. -0.0105175 0.999139 -0.0401258 -0.159048
  6881. 0.00521928 0.0401823 0.999179 -0.113326
  6882. 0 0 0 1
  6883. shift: 0.159146
  6884. -----------------------------------------------------------------
  6885. (Processed/Input): (362 / 362)
  6886. -----------------------------------------------------------------
  6887. Sequence number: 5292
  6888. Number of scan points: 7408 points.
  6889. Number of filtered scan points: 1291 points.
  6890. transformed_scan_ptr: 7408 points.
  6891. map: 6294 points.
  6892. NDT has converged: 1
  6893. Fitness score: 26.7719
  6894. Number of iteration: 4
  6895. (x,y,z,roll,pitch,yaw):
  6896. (-0.000619194, -0.11121, -0.104667, 0.0416035, 0.00104195, -0.00526943)
  6897. Transformation Matrix:
  6898. 0.999986 0.00530818 0.000821878 -0.000619194
  6899. -0.0052694 0.999121 -0.0415964 -0.11121
  6900. -0.00104195 0.0415914 0.999134 -0.104667
  6901. 0 0 0 1
  6902. shift: 0.111212
  6903. -----------------------------------------------------------------
  6904. (Processed/Input): (363 / 363)
  6905. -----------------------------------------------------------------
  6906. Sequence number: 5293
  6907. Number of scan points: 7434 points.
  6908. Number of filtered scan points: 1279 points.
  6909. transformed_scan_ptr: 7434 points.
  6910. map: 6294 points.
  6911. NDT has converged: 1
  6912. Fitness score: 26.3974
  6913. Number of iteration: 2
  6914. (x,y,z,roll,pitch,yaw):
  6915. (0.00890863, -0.0845343, -0.114645, 0.040113, 0.00398211, -0.00231175)
  6916. Transformation Matrix:
  6917. 0.999989 0.00246958 0.00388618 0.00890863
  6918. -0.00231173 0.999193 -0.0401113 -0.0845343
  6919. -0.0039821 0.0401019 0.999188 -0.114645
  6920. 0 0 0 1
  6921. shift: 0.0850024
  6922. -----------------------------------------------------------------
  6923. (Processed/Input): (364 / 364)
  6924. -----------------------------------------------------------------
  6925. Sequence number: 5294
  6926. Number of scan points: 7450 points.
  6927. Number of filtered scan points: 1252 points.
  6928. transformed_scan_ptr: 7450 points.
  6929. map: 6294 points.
  6930. NDT has converged: 1
  6931. Fitness score: 25.2452
  6932. Number of iteration: 5
  6933. (x,y,z,roll,pitch,yaw):
  6934. (0.0919992, -0.163071, -0.130888, 0.0417906, 0.0019509, 0.000792493)
  6935. Transformation Matrix:
  6936. 0.999998 -0.000710295 0.00198231 0.0919992
  6937. 0.000792492 0.999127 -0.0417769 -0.163071
  6938. -0.0019509 0.0417783 0.999125 -0.130888
  6939. 0 0 0 1
  6940. shift: 0.187233
  6941. -----------------------------------------------------------------
  6942. (Processed/Input): (365 / 365)
  6943. -----------------------------------------------------------------
  6944. Sequence number: 5295
  6945. Number of scan points: 7378 points.
  6946. Number of filtered scan points: 1235 points.
  6947. transformed_scan_ptr: 7378 points.
  6948. map: 6294 points.
  6949. NDT has converged: 1
  6950. Fitness score: 25.2457
  6951. Number of iteration: 6
  6952. (x,y,z,roll,pitch,yaw):
  6953. (0.036234, -0.204799, -0.137641, 0.0423563, -0.00240168, 0.00782822)
  6954. Transformation Matrix:
  6955. 0.999967 -0.00792281 -0.00206799 0.036234
  6956. 0.00782811 0.999072 -0.0423611 -0.204799
  6957. 0.00240168 0.0423435 0.9991 -0.137641
  6958. 0 0 0 1
  6959. shift: 0.207979
  6960. -----------------------------------------------------------------
  6961. (Processed/Input): (366 / 366)
  6962. -----------------------------------------------------------------
  6963. Sequence number: 5296
  6964. Number of scan points: 7440 points.
  6965. Number of filtered scan points: 1248 points.
  6966. transformed_scan_ptr: 7440 points.
  6967. map: 6294 points.
  6968. NDT has converged: 1
  6969. Fitness score: 25.133
  6970. Number of iteration: 4
  6971. (x,y,z,roll,pitch,yaw):
  6972. (0.0470983, -0.179008, -0.145997, 0.0464662, -0.0045119, 0.00366752)
  6973. Transformation Matrix:
  6974. 0.999983 -0.00387313 -0.00433662 0.0470983
  6975. 0.00366747 0.998913 -0.0464657 -0.179008
  6976. 0.00451188 0.046449 0.99891 -0.145997
  6977. 0 0 0 1
  6978. shift: 0.1851
  6979. -----------------------------------------------------------------
  6980. (Processed/Input): (367 / 367)
  6981. -----------------------------------------------------------------
  6982. Sequence number: 5297
  6983. Number of scan points: 7593 points.
  6984. Number of filtered scan points: 1247 points.
  6985. transformed_scan_ptr: 7593 points.
  6986. map: 6294 points.
  6987. NDT has converged: 1
  6988. Fitness score: 25.3468
  6989. Number of iteration: 6
  6990. (x,y,z,roll,pitch,yaw):
  6991. (-0.0166649, -0.23262, -0.154198, 0.0495369, -0.00634958, -0.00849287)
  6992. Transformation Matrix:
  6993. 0.999944 0.00816795 -0.00676205 -0.0166649
  6994. -0.0084926 0.99874 -0.049461 -0.23262
  6995. 0.00634953 0.0495157 0.998753 -0.154198
  6996. 0 0 0 1
  6997. shift: 0.233216
  6998. -----------------------------------------------------------------
  6999. (Processed/Input): (368 / 368)
  7000. -----------------------------------------------------------------
  7001. Sequence number: 5298
  7002. Number of scan points: 7503 points.
  7003. Number of filtered scan points: 1253 points.
  7004. transformed_scan_ptr: 7503 points.
  7005. map: 6294 points.
  7006. NDT has converged: 1
  7007. Fitness score: 24.9441
  7008. Number of iteration: 2
  7009. (x,y,z,roll,pitch,yaw):
  7010. (-0.105143, -0.268761, -0.150854, 0.048948, -0.00620553, -0.0187932)
  7011. Transformation Matrix:
  7012. 0.999804 0.018466 -0.00711642 -0.105143
  7013. -0.0187917 0.998632 -0.0488034 -0.268761
  7014. 0.00620549 0.0489275 0.998783 -0.150854
  7015. 0 0 0 1
  7016. shift: 0.288596
  7017. -----------------------------------------------------------------
  7018. (Processed/Input): (369 / 369)
  7019. -----------------------------------------------------------------
  7020. Sequence number: 5299
  7021. Number of scan points: 7449 points.
  7022. Number of filtered scan points: 1258 points.
  7023. transformed_scan_ptr: 7449 points.
  7024. map: 6294 points.
  7025. NDT has converged: 1
  7026. Fitness score: 24.8322
  7027. Number of iteration: 8
  7028. (x,y,z,roll,pitch,yaw):
  7029. (-0.035597, -0.21761, -0.153335, 0.0491581, -0.00437306, -0.0087476)
  7030. Transformation Matrix:
  7031. 0.999952 0.00852205 -0.00479743 -0.035597
  7032. -0.00874741 0.998756 -0.0490982 -0.21761
  7033. 0.00437304 0.0491378 0.998782 -0.153335
  7034. 0 0 0 1
  7035. shift: 0.220502
  7036. -----------------------------------------------------------------
  7037. (Processed/Input): (370 / 370)
  7038. -----------------------------------------------------------------
  7039. Sequence number: 5300
  7040. Number of scan points: 7576 points.
  7041. Number of filtered scan points: 1267 points.
  7042. transformed_scan_ptr: 7576 points.
  7043. map: 6294 points.
  7044. NDT has converged: 1
  7045. Fitness score: 25.4459
  7046. Number of iteration: 4
  7047. (x,y,z,roll,pitch,yaw):
  7048. (-0.0639024, -0.155917, -0.154027, 0.0476488, -0.00434585, -0.00299972)
  7049. Transformation Matrix:
  7050. 0.999986 0.00278931 -0.00448377 -0.0639024
  7051. -0.00299968 0.998861 -0.0476176 -0.155917
  7052. 0.00434584 0.0476304 0.998856 -0.154027
  7053. 0 0 0 1
  7054. shift: 0.168504
  7055. -----------------------------------------------------------------
  7056. (Processed/Input): (371 / 371)
  7057. -----------------------------------------------------------------
  7058. Sequence number: 5301
  7059. Number of scan points: 7543 points.
  7060. Number of filtered scan points: 1231 points.
  7061. transformed_scan_ptr: 7543 points.
  7062. map: 6294 points.
  7063. NDT has converged: 1
  7064. Fitness score: 25.1242
  7065. Number of iteration: 4
  7066. (x,y,z,roll,pitch,yaw):
  7067. (-0.120676, -0.0879349, -0.156096, 0.0464575, -0.00477126, -0.00526835)
  7068. Transformation Matrix:
  7069. 0.999975 0.00504106 -0.0050107 -0.120676
  7070. -0.00526826 0.998908 -0.046415 -0.0879349
  7071. 0.00477124 0.0464402 0.99891 -0.156096
  7072. 0 0 0 1
  7073. shift: 0.149316
  7074. -----------------------------------------------------------------
  7075. (Processed/Input): (372 / 372)
  7076. -----------------------------------------------------------------
  7077. Sequence number: 5302
  7078. Number of scan points: 7411 points.
  7079. Number of filtered scan points: 1217 points.
  7080. transformed_scan_ptr: 7411 points.
  7081. map: 6294 points.
  7082. NDT has converged: 1
  7083. Fitness score: 25.1293
  7084. Number of iteration: 5
  7085. (x,y,z,roll,pitch,yaw):
  7086. (-0.0753026, -0.0927558, -0.154862, 0.0458442, -0.00384315, -0.0134012)
  7087. Transformation Matrix:
  7088. 0.999903 0.0132107 -0.00445289 -0.0753026
  7089. -0.0134007 0.998862 -0.0457726 -0.0927558
  7090. 0.00384314 0.0458278 0.998942 -0.154862
  7091. 0 0 0 1
  7092. shift: 0.119474
  7093. -----------------------------------------------------------------
  7094. (Processed/Input): (373 / 373)
  7095. -----------------------------------------------------------------
  7096. Sequence number: 5303
  7097. Number of scan points: 7436 points.
  7098. Number of filtered scan points: 1184 points.
  7099. transformed_scan_ptr: 7436 points.
  7100. map: 6294 points.
  7101. NDT has converged: 1
  7102. Fitness score: 24.9931
  7103. Number of iteration: 2
  7104. (x,y,z,roll,pitch,yaw):
  7105. (-0.0592792, -0.0851028, -0.163956, 0.0466197, -0.00221784, -0.00898482)
  7106. Transformation Matrix:
  7107. 0.999957 0.00887158 -0.00263405 -0.0592792
  7108. -0.00898467 0.998874 -0.0465811 -0.0851028
  7109. 0.00221783 0.0466027 0.998911 -0.163956
  7110. 0 0 0 1
  7111. shift: 0.103714
  7112. -----------------------------------------------------------------
  7113. (Processed/Input): (374 / 374)
  7114. -----------------------------------------------------------------
  7115. Sequence number: 5304
  7116. Number of scan points: 7591 points.
  7117. Number of filtered scan points: 1178 points.
  7118. transformed_scan_ptr: 7591 points.
  7119. map: 6294 points.
  7120. NDT has converged: 1
  7121. Fitness score: 25.0847
  7122. Number of iteration: 4
  7123. (x,y,z,roll,pitch,yaw):
  7124. (0.0124994, -0.117581, -0.174502, 0.0476068, -3.70844e-05, 0.000358325)
  7125. Transformation Matrix:
  7126. 1 -0.000359684 -1.99906e-05 0.0124994
  7127. 0.000358325 0.998867 -0.0475888 -0.117581
  7128. 3.70844e-05 0.0475888 0.998867 -0.174502
  7129. 0 0 0 1
  7130. shift: 0.118244
  7131. -----------------------------------------------------------------
  7132. (Processed/Input): (375 / 375)
  7133. -----------------------------------------------------------------
  7134. Sequence number: 5305
  7135. Number of scan points: 7298 points.
  7136. Number of filtered scan points: 1161 points.
  7137. transformed_scan_ptr: 7298 points.
  7138. map: 6294 points.
  7139. NDT has converged: 1
  7140. Fitness score: 25.1264
  7141. Number of iteration: 6
  7142. (x,y,z,roll,pitch,yaw):
  7143. (-0.0136234, -0.177132, -0.189223, 0.049655, -0.000541117, -0.00836918)
  7144. Transformation Matrix:
  7145. 0.999965 0.00833191 -0.000955834 -0.0136234
  7146. -0.00836908 0.998733 -0.0496284 -0.177132
  7147. 0.000541117 0.0496346 0.998767 -0.189223
  7148. 0 0 0 1
  7149. shift: 0.177655
  7150. -----------------------------------------------------------------
  7151. (Processed/Input): (376 / 376)
  7152. -----------------------------------------------------------------
  7153. Sequence number: 5306
  7154. Number of scan points: 7259 points.
  7155. Number of filtered scan points: 1212 points.
  7156. transformed_scan_ptr: 7259 points.
  7157. map: 6294 points.
  7158. NDT has converged: 1
  7159. Fitness score: 26.3807
  7160. Number of iteration: 5
  7161. (x,y,z,roll,pitch,yaw):
  7162. (0.046401, -0.146826, -0.205453, 0.0522758, 0.0012335, -0.00119812)
  7163. Transformation Matrix:
  7164. 0.999999 0.00126094 0.00116922 0.046401
  7165. -0.00119812 0.998633 -0.0522534 -0.146826
  7166. -0.0012335 0.052252 0.998633 -0.205453
  7167. 0 0 0 1
  7168. shift: 0.153983
  7169. -----------------------------------------------------------------
  7170. (Processed/Input): (377 / 377)
  7171. -----------------------------------------------------------------
  7172. Sequence number: 5307
  7173. Number of scan points: 7311 points.
  7174. Number of filtered scan points: 1230 points.
  7175. transformed_scan_ptr: 7311 points.
  7176. map: 6294 points.
  7177. NDT has converged: 1
  7178. Fitness score: 26.238
  7179. Number of iteration: 4
  7180. (x,y,z,roll,pitch,yaw):
  7181. (0.000271412, -0.182263, -0.224077, 0.0562679, 0.00237905, -0.000982778)
  7182. Transformation Matrix:
  7183. 0.999997 0.00111502 0.00232001 0.000271412
  7184. -0.000982775 0.998417 -0.0562406 -0.182263
  7185. -0.00237905 0.0562381 0.998415 -0.224077
  7186. 0 0 0 1
  7187. shift: 0.182263
  7188. -----------------------------------------------------------------
  7189. (Processed/Input): (378 / 378)
  7190. -----------------------------------------------------------------
  7191. Sequence number: 5308
  7192. Number of scan points: 7311 points.
  7193. Number of filtered scan points: 1230 points.
  7194. transformed_scan_ptr: 7311 points.
  7195. map: 6294 points.
  7196. NDT has converged: 1
  7197. Fitness score: 26.3048
  7198. Number of iteration: 4
  7199. (x,y,z,roll,pitch,yaw):
  7200. (-0.00341836, -0.183474, -0.259251, 0.0627147, 0.00082895, -0.0100073)
  7201. Transformation Matrix:
  7202. 0.99995 0.0100394 0.000200098 -0.00341836
  7203. -0.0100071 0.997984 -0.0626787 -0.183474
  7204. -0.00082895 0.0626735 0.998034 -0.259251
  7205. 0 0 0 1
  7206. shift: 0.183506
  7207. -----------------------------------------------------------------
  7208. (Processed/Input): (379 / 379)
  7209. -----------------------------------------------------------------
  7210. Sequence number: 5309
  7211. Number of scan points: 7396 points.
  7212. Number of filtered scan points: 1240 points.
  7213. transformed_scan_ptr: 7396 points.
  7214. map: 6294 points.
  7215. NDT has converged: 1
  7216. Fitness score: 26.2591
  7217. Number of iteration: 2
  7218. (x,y,z,roll,pitch,yaw):
  7219. (0.00463925, -0.183292, -0.297473, 0.0684685, 0.0019006, -0.00831384)
  7220. Transformation Matrix:
  7221. 0.999964 0.00842429 0.0013273 0.00463925
  7222. -0.00831373 0.997621 -0.0684284 -0.183292
  7223. -0.0019006 0.0684149 0.997655 -0.297473
  7224. 0 0 0 1
  7225. shift: 0.183351
  7226. -----------------------------------------------------------------
  7227. (Processed/Input): (380 / 380)
  7228. -----------------------------------------------------------------
  7229. Sequence number: 5310
  7230. Number of scan points: 7268 points.
  7231. Number of filtered scan points: 1189 points.
  7232. transformed_scan_ptr: 7268 points.
  7233. map: 6294 points.
  7234. NDT has converged: 1
  7235. Fitness score: 26.0194
  7236. Number of iteration: 5
  7237. (x,y,z,roll,pitch,yaw):
  7238. (-0.04114, -0.223837, -0.307021, 0.0736115, 0.000959261, -0.00383922)
  7239. Transformation Matrix:
  7240. 0.999992 0.00389936 0.000674301 -0.04114
  7241. -0.00383921 0.997284 -0.0735482 -0.223837
  7242. -0.000959261 0.0735451 0.997291 -0.307021
  7243. 0 0 0 1
  7244. shift: 0.227586
  7245. -----------------------------------------------------------------
  7246. (Processed/Input): (381 / 381)
  7247. -----------------------------------------------------------------
  7248. Sequence number: 5311
  7249. Number of scan points: 7358 points.
  7250. Number of filtered scan points: 1199 points.
  7251. transformed_scan_ptr: 7358 points.
  7252. map: 6294 points.
  7253. NDT has converged: 1
  7254. Fitness score: 25.2301
  7255. Number of iteration: 5
  7256. (x,y,z,roll,pitch,yaw):
  7257. (-0.024231, -0.199113, -0.324296, 0.082577, -0.00153341, 0.00317267)
  7258. Transformation Matrix:
  7259. 0.999994 -0.00328833 -0.00126648 -0.024231
  7260. 0.00317266 0.996587 -0.0824877 -0.199113
  7261. 0.00153341 0.0824831 0.996591 -0.324296
  7262. 0 0 0 1
  7263. shift: 0.200582
  7264. -----------------------------------------------------------------
  7265. (Processed/Input): (382 / 382)
  7266. -----------------------------------------------------------------
  7267. Sequence number: 5312
  7268. Number of scan points: 7417 points.
  7269. Number of filtered scan points: 1219 points.
  7270. transformed_scan_ptr: 7417 points.
  7271. map: 6294 points.
  7272. NDT has converged: 1
  7273. Fitness score: 25.6299
  7274. Number of iteration: 3
  7275. (x,y,z,roll,pitch,yaw):
  7276. (-0.0424451, -0.156806, -0.33786, 0.0910215, -0.00299049, 0.00646719)
  7277. Transformation Matrix:
  7278. 0.999975 -0.00671219 -0.00239021 -0.0424451
  7279. 0.00646711 0.995838 -0.0909132 -0.156806
  7280. 0.00299049 0.0908954 0.995856 -0.33786
  7281. 0 0 0 1
  7282. shift: 0.162449
  7283. -----------------------------------------------------------------
  7284. (Processed/Input): (383 / 383)
  7285. -----------------------------------------------------------------
  7286. Sequence number: 5313
  7287. Number of scan points: 7418 points.
  7288. Number of filtered scan points: 1211 points.
  7289. transformed_scan_ptr: 7418 points.
  7290. map: 6294 points.
  7291. NDT has converged: 1
  7292. Fitness score: 25.0778
  7293. Number of iteration: 5
  7294. (x,y,z,roll,pitch,yaw):
  7295. (-0.00532837, -0.237193, -0.354745, 0.0985066, -0.00385497, 0.00146026)
  7296. Transformation Matrix:
  7297. 0.999991 -0.00183231 -0.00369266 -0.00532837
  7298. 0.00146025 0.995151 -0.0983528 -0.237193
  7299. 0.00385497 0.0983466 0.995145 -0.354745
  7300. 0 0 0 1
  7301. shift: 0.237253
  7302. -----------------------------------------------------------------
  7303. (Processed/Input): (384 / 384)
  7304. -----------------------------------------------------------------
  7305. Sequence number: 5314
  7306. Number of scan points: 7389 points.
  7307. Number of filtered scan points: 1186 points.
  7308. transformed_scan_ptr: 7389 points.
  7309. map: 6294 points.
  7310. NDT has converged: 1
  7311. Fitness score: 24.949
  7312. Number of iteration: 2
  7313. (x,y,z,roll,pitch,yaw):
  7314. (0.0165518, -0.278463, -0.355728, 0.103524, -0.00670592, -0.00591503)
  7315. Transformation Matrix:
  7316. 0.99996 0.00519036 -0.0072811 0.0165518
  7317. -0.00591487 0.994633 -0.103298 -0.278463
  7318. 0.00670587 0.103337 0.994624 -0.355728
  7319. 0 0 0 1
  7320. shift: 0.278955
  7321. -----------------------------------------------------------------
  7322. (Processed/Input): (385 / 385)
  7323. -----------------------------------------------------------------
  7324. Sequence number: 5315
  7325. Number of scan points: 7286 points.
  7326. Number of filtered scan points: 1213 points.
  7327. transformed_scan_ptr: 7286 points.
  7328. map: 6294 points.
  7329. NDT has converged: 1
  7330. Fitness score: 26.2941
  7331. Number of iteration: 3
  7332. (x,y,z,roll,pitch,yaw):
  7333. (-0.0306358, -0.325218, -0.362146, 0.108272, -0.00630096, -0.00812783)
  7334. Transformation Matrix:
  7335. 0.999947 0.00739929 -0.0071421 -0.0306358
  7336. -0.00812758 0.994117 -0.108006 -0.325218
  7337. 0.00630092 0.108058 0.994125 -0.362146
  7338. 0 0 0 1
  7339. shift: 0.326658
  7340. -----------------------------------------------------------------
  7341. (Processed/Input): (386 / 386)
  7342. -----------------------------------------------------------------
  7343. Sequence number: 5316
  7344. Number of scan points: 7340 points.
  7345. Number of filtered scan points: 1192 points.
  7346. transformed_scan_ptr: 7340 points.
  7347. map: 6294 points.
  7348. NDT has converged: 1
  7349. Fitness score: 26.3282
  7350. Number of iteration: 4
  7351. (x,y,z,roll,pitch,yaw):
  7352. (0.0343186, -0.28638, -0.386725, 0.106953, -0.00212845, -0.00409796)
  7353. Transformation Matrix:
  7354. 0.999989 0.00384733 -0.00255372 0.0343186
  7355. -0.00409794 0.994279 -0.10674 -0.28638
  7356. 0.00212845 0.106749 0.994284 -0.386725
  7357. 0 0 0 1
  7358. shift: 0.288428
  7359. -----------------------------------------------------------------
  7360. (Processed/Input): (387 / 387)
  7361. -----------------------------------------------------------------
  7362. Sequence number: 5317
  7363. Number of scan points: 7444 points.
  7364. Number of filtered scan points: 1156 points.
  7365. transformed_scan_ptr: 7444 points.
  7366. map: 6294 points.
  7367. NDT has converged: 1
  7368. Fitness score: 27.0649
  7369. Number of iteration: 3
  7370. (x,y,z,roll,pitch,yaw):
  7371. (0.0169402, -0.258296, -0.392524, 0.102899, 0.000785353, -0.00630856)
  7372. Transformation Matrix:
  7373. 0.99998 0.00635582 0.000133184 0.0169402
  7374. -0.00630851 0.99469 -0.10272 -0.258296
  7375. -0.000785352 0.102718 0.99471 -0.392524
  7376. 0 0 0 1
  7377. shift: 0.25885
  7378. -----------------------------------------------------------------
  7379. (Processed/Input): (388 / 388)
  7380. -----------------------------------------------------------------
  7381. Sequence number: 5318
  7382. Number of scan points: 7143 points.
  7383. Number of filtered scan points: 1180 points.
  7384. transformed_scan_ptr: 7143 points.
  7385. map: 6294 points.
  7386. NDT has converged: 1
  7387. Fitness score: 27.702
  7388. Number of iteration: 3
  7389. (x,y,z,roll,pitch,yaw):
  7390. (-0.0166999, -0.268779, -0.3439, 0.0914121, -0.00731849, -0.00229296)
  7391. Transformation Matrix:
  7392. 0.999971 0.00161533 -0.00749716 -0.0166999
  7393. -0.0022929 0.995824 -0.0912679 -0.268779
  7394. 0.00731842 0.0912824 0.995798 -0.3439
  7395. 0 0 0 1
  7396. shift: 0.269297
  7397. -----------------------------------------------------------------
  7398. (Processed/Input): (389 / 389)
  7399. -----------------------------------------------------------------
  7400. Sequence number: 5319
  7401. Number of scan points: 6793 points.
  7402. Number of filtered scan points: 1117 points.
  7403. transformed_scan_ptr: 6793 points.
  7404. map: 6294 points.
  7405. NDT has converged: 1
  7406. Fitness score: 27.7832
  7407. Number of iteration: 2
  7408. (x,y,z,roll,pitch,yaw):
  7409. (-0.0222348, -0.250805, -0.321166, 0.0867183, -0.0139296, -0.000956922)
  7410. Transformation Matrix:
  7411. 0.999903 -0.000253075 -0.0139597 -0.0222348
  7412. -0.000956829 0.996243 -0.0865963 -0.250805
  7413. 0.0139292 0.0866012 0.996146 -0.321166
  7414. 0 0 0 1
  7415. shift: 0.251788
  7416. -----------------------------------------------------------------
  7417. (Processed/Input): (390 / 390)
  7418. (Processed/Input): (391 / 391)
  7419. -----------------------------------------------------------------
  7420. Sequence number: 5320
  7421. Number of scan points: 5984 points.
  7422. Number of filtered scan points: 1067 points.
  7423. transformed_scan_ptr: 5984 points.
  7424. map: 6294 points.
  7425. NDT has converged: 1
  7426. Fitness score: 29.9431
  7427. Number of iteration: 4
  7428. (x,y,z,roll,pitch,yaw):
  7429. (-0.0423867, -0.27433, -0.311235, 0.0751318, -0.00435091, -0.00755687)
  7430. Transformation Matrix:
  7431. 0.999962 0.0072089 -0.00490572 -0.0423867
  7432. -0.00755672 0.997153 -0.0750262 -0.27433
  7433. 0.0043509 0.0750604 0.997169 -0.311235
  7434. 0 0 0 1
  7435. shift: 0.277585
  7436. -----------------------------------------------------------------
  7437. -----------------------------------------------------------------
  7438. Sequence number: 5321
  7439. Number of scan points: 6893 points.
  7440. Number of filtered scan points: 1089 points.
  7441. transformed_scan_ptr: 6893 points.
  7442. map: 6294 points.
  7443. NDT has converged: 1
  7444. Fitness score: 27.6019
  7445. Number of iteration: 6
  7446. (x,y,z,roll,pitch,yaw):
  7447. (-0.11446, -0.101019, -0.373225, 0.0584386, -0.0112716, -0.0103621)
  7448. Transformation Matrix:
  7449. 0.999883 0.00968596 -0.0118567 -0.11446
  7450. -0.0103613 0.998246 -0.0582856 -0.101019
  7451. 0.0112714 0.0584016 0.99823 -0.373225
  7452. 0 0 0 1
  7453. shift: 0.152663
  7454. -----------------------------------------------------------------
  7455. (Processed/Input): (392 / 392)
  7456. -----------------------------------------------------------------
  7457. Sequence number: 5322
  7458. Number of scan points: 7558 points.
  7459. Number of filtered scan points: 1163 points.
  7460. transformed_scan_ptr: 7558 points.
  7461. map: 6294 points.
  7462. NDT has converged: 1
  7463. Fitness score: 26.9927
  7464. Number of iteration: 7
  7465. (x,y,z,roll,pitch,yaw):
  7466. (-0.209147, -0.147651, -0.374034, 0.0404006, -0.0214658, -0.0159696)
  7467. Transformation Matrix:
  7468. 0.999642 0.0150891 -0.0220889 -0.209147
  7469. -0.0159652 0.99907 -0.040042 -0.147651
  7470. 0.0214642 0.0403803 0.998954 -0.374034
  7471. 0 0 0 1
  7472. shift: 0.256014
  7473. -----------------------------------------------------------------
  7474. (Processed/Input): (393 / 393)
  7475. -----------------------------------------------------------------
  7476. Sequence number: 5323
  7477. Number of scan points: 7724 points.
  7478. Number of filtered scan points: 1223 points.
  7479. transformed_scan_ptr: 7724 points.
  7480. map: 6294 points.
  7481. NDT has converged: 1
  7482. Fitness score: 27.2865
  7483. Number of iteration: 9
  7484. (x,y,z,roll,pitch,yaw):
  7485. (-0.0130473, -0.156128, -0.326788, 0.0442178, -0.0180348, -0.00129237)
  7486. Transformation Matrix:
  7487. 0.999837 0.000493958 -0.0180734 -0.0130473
  7488. -0.00129216 0.999023 -0.0441801 -0.156128
  7489. 0.0180339 0.0441962 0.99886 -0.326788
  7490. 0 0 0 1
  7491. shift: 0.156672
  7492. -----------------------------------------------------------------
  7493. (Processed/Input): (394 / 394)
  7494. -----------------------------------------------------------------
  7495. Sequence number: 5324
  7496. Number of scan points: 7692 points.
  7497. Number of filtered scan points: 1267 points.
  7498. transformed_scan_ptr: 7692 points.
  7499. map: 6294 points.
  7500. NDT has converged: 1
  7501. Fitness score: 27.1775
  7502. Number of iteration: 5
  7503. (x,y,z,roll,pitch,yaw):
  7504. (0.0142147, -0.154384, -0.270344, 0.0517169, -0.0156456, -0.000510661)
  7505. Transformation Matrix:
  7506. 0.999877 -0.000298764 -0.0156505 0.0142147
  7507. -0.000510598 0.998663 -0.0516858 -0.154384
  7508. 0.015645 0.0516875 0.998541 -0.270344
  7509. 0 0 0 1
  7510. shift: 0.155037
  7511. -----------------------------------------------------------------
  7512. (Processed/Input): (395 / 395)
  7513. -----------------------------------------------------------------
  7514. Sequence number: 5325
  7515. Number of scan points: 7622 points.
  7516. Number of filtered scan points: 1293 points.
  7517. transformed_scan_ptr: 7622 points.
  7518. map: 6294 points.
  7519. NDT has converged: 1
  7520. Fitness score: 27.1253
  7521. Number of iteration: 3
  7522. (x,y,z,roll,pitch,yaw):
  7523. (0.0883046, -0.113758, -0.234338, 0.0512347, -0.00981861, 0.00170944)
  7524. Transformation Matrix:
  7525. 0.99995 -0.00221002 -0.00971801 0.0883046
  7526. 0.00170936 0.998685 -0.051229 -0.113758
  7527. 0.00981845 0.0512099 0.99864 -0.234338
  7528. 0 0 0 1
  7529. shift: 0.144009
  7530. -----------------------------------------------------------------
  7531. (Processed/Input): (396 / 396)
  7532. -----------------------------------------------------------------
  7533. Sequence number: 5326
  7534. Number of scan points: 7754 points.
  7535. Number of filtered scan points: 1342 points.
  7536. transformed_scan_ptr: 7754 points.
  7537. map: 6294 points.
  7538. NDT has converged: 1
  7539. Fitness score: 28.3065
  7540. Number of iteration: 4
  7541. (x,y,z,roll,pitch,yaw):
  7542. (0.0698687, -0.145531, -0.221571, 0.0358335, -0.0118461, 0.00744212)
  7543. Transformation Matrix:
  7544. 0.999902 -0.00786166 -0.0115712 0.0698687
  7545. 0.00744153 0.999327 -0.0359129 -0.145531
  7546. 0.0118458 0.0358233 0.999288 -0.221571
  7547. 0 0 0 1
  7548. shift: 0.161434
  7549. -----------------------------------------------------------------
  7550. (Processed/Input): (397 / 397)
  7551. -----------------------------------------------------------------
  7552. Sequence number: 5327
  7553. Number of scan points: 7605 points.
  7554. Number of filtered scan points: 1354 points.
  7555. transformed_scan_ptr: 7605 points.
  7556. map: 6294 points.
  7557. NDT has converged: 1
  7558. Fitness score: 27.9515
  7559. Number of iteration: 12
  7560. (x,y,z,roll,pitch,yaw):
  7561. (-0.165245, 0.00572372, -0.195268, 0.0211202, -0.0151477, -0.0188742)
  7562. Transformation Matrix:
  7563. 0.999707 0.0185491 -0.0155396 -0.165245
  7564. -0.0188709 0.999605 -0.0208291 0.00572372
  7565. 0.0151471 0.0211162 0.999662 -0.195268
  7566. 0 0 0 1
  7567. shift: 0.165344
  7568. -----------------------------------------------------------------
  7569. (Processed/Input): (398 / 398)
  7570. -----------------------------------------------------------------
  7571. Sequence number: 5328
  7572. Number of scan points: 7691 points.
  7573. Number of filtered scan points: 1327 points.
  7574. transformed_scan_ptr: 7691 points.
  7575. map: 6294 points.
  7576. NDT has converged: 1
  7577. Fitness score: 27.6245
  7578. Number of iteration: 2
  7579. (x,y,z,roll,pitch,yaw):
  7580. (-0.368371, 0.153936, -0.159013, 0.00909705, -0.0168099, -0.053239)
  7581. Transformation Matrix:
  7582. 0.998442 0.053059 -0.0172687 -0.368371
  7583. -0.0532064 0.99855 -0.00818959 0.153936
  7584. 0.0168091 0.00909564 0.999817 -0.159013
  7585. 0 0 0 1
  7586. shift: 0.399241
  7587. -----------------------------------------------------------------
  7588. (Processed/Input): (399 / 399)
  7589. -----------------------------------------------------------------
  7590. Sequence number: 5329
  7591. Number of scan points: 7830 points.
  7592. Number of filtered scan points: 1377 points.
  7593. transformed_scan_ptr: 7830 points.
  7594. map: 6294 points.
  7595. NDT has converged: 1
  7596. Fitness score: 27.344
  7597. Number of iteration: 6
  7598. (x,y,z,roll,pitch,yaw):
  7599. (-0.348342, 0.246358, -0.113059, 0.00114739, -0.00546759, -0.0680588)
  7600. Transformation Matrix:
  7601. 0.99767 0.0679999 -0.00553294 -0.348342
  7602. -0.0680052 0.997685 -0.000772909 0.246358
  7603. 0.00546757 0.00114738 0.999984 -0.113059
  7604. 0 0 0 1
  7605. shift: 0.426654
  7606. -----------------------------------------------------------------
  7607. (Processed/Input): (400 / 400)
  7608. -----------------------------------------------------------------
  7609. Sequence number: 5330
  7610. Number of scan points: 7697 points.
  7611. Number of filtered scan points: 1361 points.
  7612. transformed_scan_ptr: 7697 points.
  7613. map: 6294 points.
  7614. NDT has converged: 1
  7615. Fitness score: 27.6019
  7616. Number of iteration: 6
  7617. (x,y,z,roll,pitch,yaw):
  7618. (-0.185508, 0.368252, -0.0909879, 0.000947943, 0.00600624, -0.0792927)
  7619. Transformation Matrix:
  7620. 0.99684 0.0792153 0.00591225 -0.185508
  7621. -0.0792083 0.996857 -0.00142072 0.368252
  7622. -0.00600621 0.000947926 0.999982 -0.0909879
  7623. 0 0 0 1
  7624. shift: 0.412338
  7625. -----------------------------------------------------------------
  7626. (Processed/Input): (401 / 401)
  7627. -----------------------------------------------------------------
  7628. Sequence number: 5331
  7629. Number of scan points: 7740 points.
  7630. Number of filtered scan points: 1357 points.
  7631. transformed_scan_ptr: 7740 points.
  7632. map: 6294 points.
  7633. NDT has converged: 1
  7634. Fitness score: 27.6466
  7635. Number of iteration: 10
  7636. (x,y,z,roll,pitch,yaw):
  7637. (-0.183823, 0.301894, -0.0894599, 0.00217087, 0.00901698, -0.096706)
  7638. Transformation Matrix:
  7639. 0.995287 0.0965746 0.0087651 -0.183823
  7640. -0.0965514 0.995323 -0.00303134 0.301894
  7641. -0.00901686 0.00217078 0.999957 -0.0894599
  7642. 0 0 0 1
  7643. shift: 0.353455
  7644. -----------------------------------------------------------------
  7645. (Processed/Input): (402 / 402)
  7646. -----------------------------------------------------------------
  7647. Sequence number: 5332
  7648. Number of scan points: 7782 points.
  7649. Number of filtered scan points: 1363 points.
  7650. transformed_scan_ptr: 7782 points.
  7651. map: 6294 points.
  7652. NDT has converged: 1
  7653. Fitness score: 27.3726
  7654. Number of iteration: 5
  7655. (x,y,z,roll,pitch,yaw):
  7656. (-0.15064, 0.178696, -0.104569, -0.0026813, 0.0129893, -0.0964126)
  7657. Transformation Matrix:
  7658. 0.995272 0.0962283 0.0131866 -0.15064
  7659. -0.0962552 0.995356 0.0014185 0.178696
  7660. -0.0129889 -0.00268107 0.999912 -0.104569
  7661. 0 0 0 1
  7662. shift: 0.233719
  7663. -----------------------------------------------------------------
  7664. (Processed/Input): (403 / 403)
  7665. -----------------------------------------------------------------
  7666. Sequence number: 5333
  7667. Number of scan points: 7763 points.
  7668. Number of filtered scan points: 1371 points.
  7669. transformed_scan_ptr: 7763 points.
  7670. map: 6294 points.
  7671. NDT has converged: 1
  7672. Fitness score: 26.459
  7673. Number of iteration: 4
  7674. (x,y,z,roll,pitch,yaw):
  7675. (-0.131219, 0.121442, -0.13412, -0.00921155, 0.0233752, -0.0990807)
  7676. Transformation Matrix:
  7677. 0.994824 0.0987003 0.0241687 -0.131219
  7678. -0.0988917 0.995075 0.0068543 0.121442
  7679. -0.0233731 -0.0092089 0.999684 -0.13412
  7680. 0 0 0 1
  7681. shift: 0.178792
  7682. -----------------------------------------------------------------
  7683. (Processed/Input): (404 / 404)
  7684. -----------------------------------------------------------------
  7685. Sequence number: 5334
  7686. Number of scan points: 7587 points.
  7687. Number of filtered scan points: 1342 points.
  7688. transformed_scan_ptr: 7587 points.
  7689. map: 6294 points.
  7690. NDT has converged: 1
  7691. Fitness score: 26.2326
  7692. Number of iteration: 2
  7693. (x,y,z,roll,pitch,yaw):
  7694. (-0.116104, 0.0429939, -0.170554, -0.00623488, 0.0265824, -0.0951703)
  7695. Transformation Matrix:
  7696. 0.995123 0.0948599 0.0270509 -0.116104
  7697. -0.0949931 0.995471 0.00368092 0.0429939
  7698. -0.0265792 -0.00623264 0.999627 -0.170554
  7699. 0 0 0 1
  7700. shift: 0.123809
  7701. -----------------------------------------------------------------
  7702. (Processed/Input): (405 / 405)
  7703. -----------------------------------------------------------------
  7704. Sequence number: 5335
  7705. Number of scan points: 7631 points.
  7706. Number of filtered scan points: 1335 points.
  7707. transformed_scan_ptr: 7631 points.
  7708. map: 6294 points.
  7709. NDT has converged: 1
  7710. Fitness score: 25.2382
  7711. Number of iteration: 9
  7712. (x,y,z,roll,pitch,yaw):
  7713. (-0.189573, 0.218267, -0.205939, 0.0210675, 0.00774806, -0.0765324)
  7714. Transformation Matrix:
  7715. 0.997043 0.0766035 0.00611293 -0.189573
  7716. -0.0764555 0.996839 -0.0215966 0.218267
  7717. -0.00774798 0.0210654 0.999748 -0.205939
  7718. 0 0 0 1
  7719. shift: 0.289099
  7720. -----------------------------------------------------------------
  7721. (Processed/Input): (406 / 406)
  7722. -----------------------------------------------------------------
  7723. Sequence number: 5336
  7724. Number of scan points: 7552 points.
  7725. Number of filtered scan points: 1287 points.
  7726. transformed_scan_ptr: 7552 points.
  7727. map: 6294 points.
  7728. NDT has converged: 1
  7729. Fitness score: 25.6487
  7730. Number of iteration: 6
  7731. (x,y,z,roll,pitch,yaw):
  7732. (-0.193114, 0.274085, -0.182912, 0.0437624, -0.00771976, -0.0755916)
  7733. Transformation Matrix:
  7734. 0.997115 0.0751106 -0.0109941 -0.193114
  7735. -0.0755174 0.996215 -0.0430411 0.274085
  7736. 0.00771968 0.0437471 0.999013 -0.182912
  7737. 0 0 0 1
  7738. shift: 0.335284
  7739. -----------------------------------------------------------------
  7740. (Processed/Input): (407 / 407)
  7741. -----------------------------------------------------------------
  7742. Sequence number: 5337
  7743. Number of scan points: 7645 points.
  7744. Number of filtered scan points: 1291 points.
  7745. transformed_scan_ptr: 7645 points.
  7746. map: 6294 points.
  7747. NDT has converged: 1
  7748. Fitness score: 25.9745
  7749. Number of iteration: 3
  7750. (x,y,z,roll,pitch,yaw):
  7751. (-0.223587, 0.264761, -0.148686, 0.0463249, -0.00657297, -0.0748611)
  7752. Transformation Matrix:
  7753. 0.997178 0.0744074 -0.010011 -0.223587
  7754. -0.0747896 0.996152 -0.0456875 0.264761
  7755. 0.00657293 0.0463073 0.998906 -0.148686
  7756. 0 0 0 1
  7757. shift: 0.346539
  7758. -----------------------------------------------------------------
  7759. (Processed/Input): (408 / 408)
  7760. -----------------------------------------------------------------
  7761. Sequence number: 5338
  7762. Number of scan points: 7612 points.
  7763. Number of filtered scan points: 1280 points.
  7764. transformed_scan_ptr: 7612 points.
  7765. map: 6294 points.
  7766. NDT has converged: 1
  7767. Fitness score: 25.5773
  7768. Number of iteration: 3
  7769. (x,y,z,roll,pitch,yaw):
  7770. (-0.258038, 0.288578, -0.124552, 0.0363832, 0.000716376, -0.0796075)
  7771. Transformation Matrix:
  7772. 0.996833 0.0794968 -0.00217906 -0.258038
  7773. -0.0795234 0.996171 -0.0363169 0.288578
  7774. -0.000716376 0.0363752 0.999338 -0.124552
  7775. 0 0 0 1
  7776. shift: 0.387119
  7777. -----------------------------------------------------------------
  7778. (Processed/Input): (409 / 409)
  7779. -----------------------------------------------------------------
  7780. Sequence number: 5339
  7781. Number of scan points: 7688 points.
  7782. Number of filtered scan points: 1273 points.
  7783. transformed_scan_ptr: 7688 points.
  7784. map: 6294 points.
  7785. NDT has converged: 1
  7786. Fitness score: 26.2242
  7787. Number of iteration: 5
  7788. (x,y,z,roll,pitch,yaw):
  7789. (-0.2472, 0.249524, -0.110988, 0.0237477, 0.00504083, -0.0848053)
  7790. Transformation Matrix:
  7791. 0.996394 0.084799 0.00300995 -0.2472
  7792. -0.0847026 0.996115 -0.024087 0.249524
  7793. -0.00504081 0.0237452 0.999705 -0.110988
  7794. 0 0 0 1
  7795. shift: 0.35124
  7796. -----------------------------------------------------------------
  7797. (Processed/Input): (410 / 410)
  7798. -----------------------------------------------------------------
  7799. Sequence number: 5340
  7800. Number of scan points: 7735 points.
  7801. Number of filtered scan points: 1229 points.
  7802. transformed_scan_ptr: 7735 points.
  7803. map: 6294 points.
  7804. NDT has converged: 1
  7805. Fitness score: 26.0101
  7806. Number of iteration: 2
  7807. (x,y,z,roll,pitch,yaw):
  7808. (-0.221198, 0.202744, -0.104329, 0.0206167, 0.00706665, -0.0841385)
  7809. Transformation Matrix:
  7810. 0.996438 0.0841665 0.00530761 -0.221198
  7811. -0.0840371 0.996238 -0.0211361 0.202744
  7812. -0.00706659 0.0206147 0.999763 -0.104329
  7813. 0 0 0 1
  7814. shift: 0.300056
  7815. -----------------------------------------------------------------
  7816. (Processed/Input): (411 / 411)
  7817. -----------------------------------------------------------------
  7818. Sequence number: 5341
  7819. Number of scan points: 7697 points.
  7820. Number of filtered scan points: 1254 points.
  7821. transformed_scan_ptr: 7697 points.
  7822. map: 6294 points.
  7823. NDT has converged: 1
  7824. Fitness score: 26.4879
  7825. Number of iteration: 2
  7826. (x,y,z,roll,pitch,yaw):
  7827. (-0.195277, 0.15594, -0.0976706, 0.0206083, 0.00707255, -0.0834871)
  7828. Transformation Matrix:
  7829. 0.996492 0.0835177 0.00532794 -0.195277
  7830. -0.0833881 0.996293 -0.0211247 0.15594
  7831. -0.00707249 0.0206063 0.999763 -0.0976706
  7832. 0 0 0 1
  7833. shift: 0.249901
  7834. -----------------------------------------------------------------
  7835. (Processed/Input): (412 / 412)
  7836. -----------------------------------------------------------------
  7837. Sequence number: 5342
  7838. Number of scan points: 7693 points.
  7839. Number of filtered scan points: 1224 points.
  7840. transformed_scan_ptr: 7693 points.
  7841. map: 6294 points.
  7842. NDT has converged: 1
  7843. Fitness score: 26.3092
  7844. Number of iteration: 11
  7845. (x,y,z,roll,pitch,yaw):
  7846. (-0.0915001, 0.275002, -0.102562, 0.0207943, 0.00905369, -0.0689815)
  7847. Transformation Matrix:
  7848. 0.997581 0.0690997 0.0075969 -0.0915001
  7849. -0.068924 0.997393 -0.0213673 0.275002
  7850. -0.00905356 0.020792 0.999743 -0.102562
  7851. 0 0 0 1
  7852. shift: 0.289825
  7853. -----------------------------------------------------------------
  7854. (Processed/Input): (413 / 413)
  7855. -----------------------------------------------------------------
  7856. Sequence number: 5343
  7857. Number of scan points: 7791 points.
  7858. Number of filtered scan points: 1218 points.
  7859. transformed_scan_ptr: 7791 points.
  7860. map: 6294 points.
  7861. NDT has converged: 1
  7862. Fitness score: 24.9908
  7863. Number of iteration: 13
  7864. (x,y,z,roll,pitch,yaw):
  7865. (0.376144, 0.271669, -0.107494, 0.019919, 0.0125524, -0.0771592)
  7866. Transformation Matrix:
  7867. 0.996946 0.0773167 0.0109769 0.376144
  7868. -0.0770766 0.996808 -0.0208258 0.271669
  7869. -0.0125521 0.0199161 0.999723 -0.107494
  7870. 0 0 0 1
  7871. shift: 0.463992
  7872. -----------------------------------------------------------------
  7873. (Processed/Input): (414 / 414)
  7874. -----------------------------------------------------------------
  7875. Sequence number: 5344
  7876. Number of scan points: 7733 points.
  7877. Number of filtered scan points: 1198 points.
  7878. transformed_scan_ptr: 7733 points.
  7879. map: 6294 points.
  7880. NDT has converged: 1
  7881. Fitness score: 24.4634
  7882. Number of iteration: 10
  7883. (x,y,z,roll,pitch,yaw):
  7884. (0.381135, 0.199073, -0.114097, 0.016813, 0.0133252, -0.0645016)
  7885. Transformation Matrix:
  7886. 0.997832 0.0646713 0.0122116 0.381135
  7887. -0.0644512 0.997765 -0.017636 0.199073
  7888. -0.0133248 0.0168107 0.99977 -0.114097
  7889. 0 0 0 1
  7890. shift: 0.429993
  7891. -----------------------------------------------------------------
  7892. (Processed/Input): (415 / 415)
  7893. -----------------------------------------------------------------
  7894. Sequence number: 5345
  7895. Number of scan points: 7789 points.
  7896. Number of filtered scan points: 1166 points.
  7897. transformed_scan_ptr: 7789 points.
  7898. map: 6294 points.
  7899. NDT has converged: 1
  7900. Fitness score: 23.7508
  7901. Number of iteration: 2
  7902. (x,y,z,roll,pitch,yaw):
  7903. (0.372133, 0.120088, -0.113862, 0.0131314, 0.0137308, -0.0536644)
  7904. Transformation Matrix:
  7905. 0.998466 0.053814 0.0130051 0.372133
  7906. -0.0536336 0.998465 -0.0138486 0.120088
  7907. -0.0137304 0.0131298 0.99982 -0.113862
  7908. 0 0 0 1
  7909. shift: 0.39103
  7910. -----------------------------------------------------------------
  7911. (Processed/Input): (416 / 416)
  7912. -----------------------------------------------------------------
  7913. Sequence number: 5346
  7914. Number of scan points: 7660 points.
  7915. Number of filtered scan points: 1158 points.
  7916. transformed_scan_ptr: 7660 points.
  7917. map: 6294 points.
  7918. NDT has converged: 1
  7919. Fitness score: 23.7116
  7920. Number of iteration: 12
  7921. (x,y,z,roll,pitch,yaw):
  7922. (0.0363866, -0.0595365, -0.120868, 0.0126744, 0.0111505, -0.00475104)
  7923. Transformation Matrix:
  7924. 0.999927 0.00489193 0.011089 0.0363866
  7925. -0.00475072 0.999908 -0.0127268 -0.0595365
  7926. -0.0111503 0.0126732 0.999858 -0.120868
  7927. 0 0 0 1
  7928. shift: 0.0697752
  7929. -----------------------------------------------------------------
  7930. (Processed/Input): (417 / 417)
  7931. -----------------------------------------------------------------
  7932. Sequence number: 5347
  7933. Number of scan points: 7634 points.
  7934. Number of filtered scan points: 1145 points.
  7935. transformed_scan_ptr: 7634 points.
  7936. map: 6294 points.
  7937. NDT has converged: 1
  7938. Fitness score: 23.7095
  7939. Number of iteration: 9
  7940. (x,y,z,roll,pitch,yaw):
  7941. (-0.136153, -0.385063, -0.117474, 0.00255598, 0.00693744, 0.0693479)
  7942. Transformation Matrix:
  7943. 0.997572 -0.0692744 0.0070978 -0.136153
  7944. 0.0692907 0.997594 -0.00206914 -0.385063
  7945. -0.00693738 0.00255592 0.999973 -0.117474
  7946. 0 0 0 1
  7947. shift: 0.408425
  7948. -----------------------------------------------------------------
  7949. (Processed/Input): (418 / 418)
  7950. -----------------------------------------------------------------
  7951. Sequence number: 5348
  7952. Number of scan points: 7825 points.
  7953. Number of filtered scan points: 1167 points.
  7954. transformed_scan_ptr: 7825 points.
  7955. map: 6294 points.
  7956. NDT has converged: 1
  7957. Fitness score: 24.6382
  7958. Number of iteration: 30
  7959. (x,y,z,roll,pitch,yaw):
  7960. (-0.114384, -0.0282907, -0.113555, -0.00415411, 0.00589696, 0.165612)
  7961. Transformation Matrix:
  7962. 0.9863 -0.164879 0.00513136 -0.114384
  7963. 0.164853 0.986305 0.0050694 -0.0282907
  7964. -0.00589692 -0.00415402 0.999974 -0.113555
  7965. 0 0 0 1
  7966. shift: 0.11783
  7967. -----------------------------------------------------------------
  7968. (Processed/Input): (419 / 419)
  7969. -----------------------------------------------------------------
  7970. Sequence number: 5349
  7971. Number of scan points: 7728 points.
  7972. Number of filtered scan points: 1133 points.
  7973. transformed_scan_ptr: 7728 points.
  7974. map: 6294 points.
  7975. NDT has converged: 1
  7976. Fitness score: 24.4489
  7977. Number of iteration: 14
  7978. (x,y,z,roll,pitch,yaw):
  7979. (-0.212984, -0.0513075, -0.112509, -0.00949535, 0.0104434, 0.28335)
  7980. Transformation Matrix:
  7981. 0.960072 -0.279656 0.00737175 -0.212984
  7982. 0.279558 0.960053 0.0120361 -0.0513075
  7983. -0.0104432 -0.00949469 0.9999 -0.112509
  7984. 0 0 0 1
  7985. shift: 0.219077
  7986. -----------------------------------------------------------------
  7987. (Processed/Input): (420 / 420)
  7988. -----------------------------------------------------------------
  7989. Sequence number: 5350
  7990. Number of scan points: 7834 points.
  7991. Number of filtered scan points: 1129 points.
  7992. transformed_scan_ptr: 7834 points.
  7993. map: 6294 points.
  7994. NDT has converged: 1
  7995. Fitness score: 24.5459
  7996. Number of iteration: 8
  7997. (x,y,z,roll,pitch,yaw):
  7998. (-0.192809, -0.135761, -0.106899, -0.0167854, 0.0155834, 0.387374)
  7999. Transformation Matrix:
  8000. 0.925792 -0.377948 0.00808555 -0.192809
  8001. 0.377713 0.925675 0.0214266 -0.135761
  8002. -0.0155827 -0.0167826 0.999738 -0.106899
  8003. 0 0 0 1
  8004. shift: 0.23581
  8005. -----------------------------------------------------------------
  8006. (Processed/Input): (421 / 421)
  8007. -----------------------------------------------------------------
  8008. Sequence number: 5351
  8009. Number of scan points: 7774 points.
  8010. Number of filtered scan points: 1112 points.
  8011. transformed_scan_ptr: 7774 points.
  8012. map: 6294 points.
  8013. NDT has converged: 1
  8014. Fitness score: 24.0012
  8015. Number of iteration: 5
  8016. (x,y,z,roll,pitch,yaw):
  8017. (0.0074286, -0.129102, -0.115454, -0.0183978, 0.0175921, 0.486314)
  8018. Transformation Matrix:
  8019. 0.883925 -0.467578 0.00695095 0.0074286
  8020. 0.467298 0.883761 0.0244841 -0.129102
  8021. -0.0175912 -0.018394 0.999676 -0.115454
  8022. 0 0 0 1
  8023. shift: 0.129315
  8024. -----------------------------------------------------------------
  8025. (Processed/Input): (422 / 422)
  8026. -----------------------------------------------------------------
  8027. Sequence number: 5352
  8028. Number of scan points: 7679 points.
  8029. Number of filtered scan points: 1117 points.
  8030. transformed_scan_ptr: 7679 points.
  8031. map: 6294 points.
  8032. NDT has converged: 1
  8033. Fitness score: 23.8321
  8034. Number of iteration: 9
  8035. (x,y,z,roll,pitch,yaw):
  8036. (0.0334865, -0.0393233, -0.116594, -0.0132082, 0.0171397, 0.561952)
  8037. Transformation Matrix:
  8038. 0.846092 -0.532984 0.00746425 0.0334865
  8039. 0.532761 0.846022 0.0203081 -0.0393233
  8040. -0.0171388 -0.0132059 0.999766 -0.116594
  8041. 0 0 0 1
  8042. shift: 0.0516495
  8043. -----------------------------------------------------------------
  8044. (Processed/Input): (423 / 423)
  8045. -----------------------------------------------------------------
  8046. Sequence number: 5353
  8047. Number of scan points: 7721 points.
  8048. Number of filtered scan points: 1106 points.
  8049. transformed_scan_ptr: 7721 points.
  8050. map: 6294 points.
  8051. NDT has converged: 1
  8052. Fitness score: 24.0786
  8053. Number of iteration: 4
  8054. (x,y,z,roll,pitch,yaw):
  8055. (0.0832411, 0.0808116, -0.114032, -0.00340595, 0.0114249, 0.650445)
  8056. Transformation Matrix:
  8057. 0.795762 -0.605568 0.00702939 0.0832411
  8058. 0.605501 0.795786 0.00962854 0.0808116
  8059. -0.0114246 -0.00340572 0.999929 -0.114032
  8060. 0 0 0 1
  8061. shift: 0.116016
  8062. -----------------------------------------------------------------
  8063. (Processed/Input): (424 / 424)
  8064. -----------------------------------------------------------------
  8065. Sequence number: 5354
  8066. Number of scan points: 7579 points.
  8067. Number of filtered scan points: 1124 points.
  8068. transformed_scan_ptr: 7579 points.
  8069. map: 6294 points.
  8070. NDT has converged: 1
  8071. Fitness score: 23.7019
  8072. Number of iteration: 7
  8073. (x,y,z,roll,pitch,yaw):
  8074. (0.161138, 0.282898, -0.103892, 0.00053645, 0.00029883, 0.721962)
  8075. Transformation Matrix:
  8076. 0.750511 -0.660858 0.000578793 0.161138
  8077. 0.660858 0.750511 -0.000205127 0.282898
  8078. -0.00029883 0.00053645 1 -0.103892
  8079. 0 0 0 1
  8080. shift: 0.325572
  8081. -----------------------------------------------------------------
  8082. (Processed/Input): (425 / 425)
  8083. -----------------------------------------------------------------
  8084. Sequence number: 5355
  8085. Number of scan points: 7718 points.
  8086. Number of filtered scan points: 1109 points.
  8087. transformed_scan_ptr: 7718 points.
  8088. map: 6294 points.
  8089. NDT has converged: 1
  8090. Fitness score: 22.8194
  8091. Number of iteration: 6
  8092. (x,y,z,roll,pitch,yaw):
  8093. (0.153545, 0.417036, -0.102549, 0.00223669, -0.0117923, 0.814952)
  8094. Transformation Matrix:
  8095. 0.685856 -0.727709 -0.00646056 0.153545
  8096. 0.727642 0.685882 -0.0101151 0.417036
  8097. 0.0117921 0.00223654 0.999928 -0.102549
  8098. 0 0 0 1
  8099. shift: 0.444404
  8100. -----------------------------------------------------------------
  8101. (Processed/Input): (426 / 426)
  8102. -----------------------------------------------------------------
  8103. Sequence number: 5356
  8104. Number of scan points: 7712 points.
  8105. Number of filtered scan points: 1116 points.
  8106. transformed_scan_ptr: 7712 points.
  8107. map: 6294 points.
  8108. NDT has converged: 1
  8109. Fitness score: 24.3309
  8110. Number of iteration: 14
  8111. (x,y,z,roll,pitch,yaw):
  8112. (0.167647, 0.120615, -0.09306, 0.00864881, -0.0191961, 0.902807)
  8113. Transformation Matrix:
  8114. 0.619295 -0.785142 -0.00509921 0.167647
  8115. 0.784924 0.619256 -0.0204258 0.120615
  8116. 0.0191949 0.00864711 0.999778 -0.09306
  8117. 0 0 0 1
  8118. shift: 0.206527
  8119. -----------------------------------------------------------------
  8120. (Processed/Input): (427 / 427)
  8121. -----------------------------------------------------------------
  8122. Sequence number: 5357
  8123. Number of scan points: 7697 points.
  8124. Number of filtered scan points: 1115 points.
  8125. transformed_scan_ptr: 7697 points.
  8126. map: 6294 points.
  8127. NDT has converged: 1
  8128. Fitness score: 23.6614
  8129. Number of iteration: 6
  8130. (x,y,z,roll,pitch,yaw):
  8131. (0.0911674, -0.0892255, -0.0762434, 0.00829932, -0.0213044, 0.978562)
  8132. Transformation Matrix:
  8133. 0.55809 -0.829765 -0.00500532 0.0911674
  8134. 0.829507 0.558051 -0.022307 -0.0892255
  8135. 0.0213028 0.00829734 0.999739 -0.0762434
  8136. 0 0 0 1
  8137. shift: 0.127564
  8138. -----------------------------------------------------------------
  8139. (Processed/Input): (428 / 428)
  8140. -----------------------------------------------------------------
  8141. Sequence number: 5358
  8142. Number of scan points: 7717 points.
  8143. Number of filtered scan points: 1104 points.
  8144. transformed_scan_ptr: 7717 points.
  8145. map: 6294 points.
  8146. NDT has converged: 1
  8147. Fitness score: 23.3478
  8148. Number of iteration: 2
  8149. (x,y,z,roll,pitch,yaw):
  8150. (-0.00632107, -0.296766, -0.0510121, 0.0081852, -0.0226997, 1.05875)
  8151. Transformation Matrix:
  8152. 0.489834 -0.871807 -0.00398527 -0.00632107
  8153. 0.871521 0.489781 -0.0237964 -0.296766
  8154. 0.0226977 0.008183 0.999709 -0.0510121
  8155. 0 0 0 1
  8156. shift: 0.296833
  8157. -----------------------------------------------------------------
  8158. (Processed/Input): (429 / 429)
  8159. -----------------------------------------------------------------
  8160. Sequence number: 5359
  8161. Number of scan points: 7720 points.
  8162. Number of filtered scan points: 1126 points.
  8163. transformed_scan_ptr: 7720 points.
  8164. map: 6294 points.
  8165. NDT has converged: 1
  8166. Fitness score: 23.6074
  8167. Number of iteration: 11
  8168. (x,y,z,roll,pitch,yaw):
  8169. (-0.0746878, -0.180207, -0.0373959, 0.00506008, -0.0210402, 1.10075)
  8170. Transformation Matrix:
  8171. 0.452829 -0.891583 -0.0050176 -0.0746878
  8172. 0.891349 0.452828 -0.0210485 -0.180207
  8173. 0.0210386 0.00505894 0.999766 -0.0373959
  8174. 0 0 0 1
  8175. shift: 0.195071
  8176. -----------------------------------------------------------------
  8177. (Processed/Input): (430 / 430)
  8178. -----------------------------------------------------------------
  8179. Sequence number: 5360
  8180. Number of scan points: 7806 points.
  8181. Number of filtered scan points: 1099 points.
  8182. transformed_scan_ptr: 7806 points.
  8183. map: 6294 points.
  8184. NDT has converged: 1
  8185. Fitness score: 23.6069
  8186. Number of iteration: 2
  8187. (x,y,z,roll,pitch,yaw):
  8188. (-0.143547, -0.0585536, -0.0227612, 9.50338e-05, -0.0181475, 1.1342)
  8189. Transformation Matrix:
  8190. 0.422785 -0.906198 -0.00758719 -0.143547
  8191. 0.906048 0.422853 -0.0164845 -0.0585536
  8192. 0.0181465 9.50182e-05 0.999835 -0.0227612
  8193. 0 0 0 1
  8194. shift: 0.15503
  8195. -----------------------------------------------------------------
  8196. (Processed/Input): (431 / 431)
  8197. -----------------------------------------------------------------
  8198. Sequence number: 5361
  8199. Number of scan points: 7780 points.
  8200. Number of filtered scan points: 1111 points.
  8201. transformed_scan_ptr: 7780 points.
  8202. map: 6294 points.
  8203. NDT has converged: 1
  8204. Fitness score: 24.2265
  8205. Number of iteration: 5
  8206. (x,y,z,roll,pitch,yaw):
  8207. (-0.256996, 0.0402603, -0.015658, -0.00476131, -0.0157176, 1.1528)
  8208. Transformation Matrix:
  8209. 0.405882 -0.913862 -0.0107313 -0.256996
  8210. 0.91379 0.405996 -0.0124309 0.0402603
  8211. 0.015717 -0.00476071 0.999865 -0.015658
  8212. 0 0 0 1
  8213. shift: 0.26013
  8214. -----------------------------------------------------------------
  8215. (Processed/Input): (432 / 432)
  8216. -----------------------------------------------------------------
  8217. Sequence number: 5362
  8218. Number of scan points: 7803 points.
  8219. Number of filtered scan points: 1106 points.
  8220. transformed_scan_ptr: 7803 points.
  8221. map: 6294 points.
  8222. NDT has converged: 1
  8223. Fitness score: 24.5569
  8224. Number of iteration: 4
  8225. (x,y,z,roll,pitch,yaw):
  8226. (-0.297008, 0.148344, -0.0102422, -0.00870434, -0.0141702, 1.15344)
  8227. Transformation Matrix:
  8228. 0.4053 -0.914081 -0.0137005 -0.297008
  8229. 0.914074 0.405438 -0.00942483 0.148344
  8230. 0.0141698 -0.00870335 0.999862 -0.0102422
  8231. 0 0 0 1
  8232. shift: 0.331993
  8233. -----------------------------------------------------------------
  8234. (Processed/Input): (433 / 433)
  8235. -----------------------------------------------------------------
  8236. Sequence number: 5363
  8237. Number of scan points: 7831 points.
  8238. Number of filtered scan points: 1117 points.
  8239. transformed_scan_ptr: 7831 points.
  8240. map: 6294 points.
  8241. NDT has converged: 1
  8242. Fitness score: 24.1066
  8243. Number of iteration: 7
  8244. (x,y,z,roll,pitch,yaw):
  8245. (-0.268571, 0.0916317, -0.0096754, -0.0107421, -0.0137181, 1.1328)
  8246. Transformation Matrix:
  8247. 0.424083 -0.90549 -0.0155456 -0.268571
  8248. 0.905519 0.424232 -0.00786622 0.0916317
  8249. 0.0137177 -0.0107409 0.999848 -0.0096754
  8250. 0 0 0 1
  8251. shift: 0.283772
  8252. -----------------------------------------------------------------
  8253. (Processed/Input): (434 / 434)
  8254. -----------------------------------------------------------------
  8255. Sequence number: 5364
  8256. Number of scan points: 7818 points.
  8257. Number of filtered scan points: 1136 points.
  8258. transformed_scan_ptr: 7818 points.
  8259. map: 6294 points.
  8260. NDT has converged: 1
  8261. Fitness score: 24.3474
  8262. Number of iteration: 5
  8263. (x,y,z,roll,pitch,yaw):
  8264. (-0.117914, 0.00558036, -0.0101565, -0.0137317, -0.0129893, 1.10901)
  8265. Transformation Matrix:
  8266. 0.445511 -0.895094 -0.0180797 -0.117914
  8267. 0.895182 0.445666 -0.00550946 0.00558036
  8268. 0.012989 -0.0137301 0.999821 -0.0101565
  8269. 0 0 0 1
  8270. shift: 0.118046
  8271. -----------------------------------------------------------------
  8272. (Processed/Input): (435 / 435)
  8273. -----------------------------------------------------------------
  8274. Sequence number: 5365
  8275. Number of scan points: 7822 points.
  8276. Number of filtered scan points: 1127 points.
  8277. transformed_scan_ptr: 7822 points.
  8278. map: 6294 points.
  8279. NDT has converged: 1
  8280. Fitness score: 24.342
  8281. Number of iteration: 2
  8282. (x,y,z,roll,pitch,yaw):
  8283. (0.01453, -0.0822427, -0.0121266, -0.0143302, -0.0145031, 1.09486)
  8284. Transformation Matrix:
  8285. 0.458125 -0.888676 -0.0193811 0.01453
  8286. 0.888769 0.458311 -0.00632398 -0.0822427
  8287. 0.0145026 -0.0143282 0.999792 -0.0121266
  8288. 0 0 0 1
  8289. shift: 0.0835164
  8290. -----------------------------------------------------------------
  8291. (Processed/Input): (436 / 436)
  8292. -----------------------------------------------------------------
  8293. Sequence number: 5366
  8294. Number of scan points: 7832 points.
  8295. Number of filtered scan points: 1119 points.
  8296. transformed_scan_ptr: 7832 points.
  8297. map: 6294 points.
  8298. NDT has converged: 1
  8299. Fitness score: 23.6973
  8300. Number of iteration: 4
  8301. (x,y,z,roll,pitch,yaw):
  8302. (0.100815, -0.175179, -0.0166982, -0.0155987, -0.0167408, 1.08619)
  8303. Transformation Matrix:
  8304. 0.465798 -0.884627 -0.0215997 0.100815
  8305. 0.884732 0.466038 -0.00754411 -0.175179
  8306. 0.01674 -0.0155959 0.999738 -0.0166982
  8307. 0 0 0 1
  8308. shift: 0.202117
  8309. -----------------------------------------------------------------
  8310. (Processed/Input): (437 / 437)
  8311. -----------------------------------------------------------------
  8312. Sequence number: 5367
  8313. Number of scan points: 7826 points.
  8314. Number of filtered scan points: 1117 points.
  8315. transformed_scan_ptr: 7826 points.
  8316. map: 6294 points.
  8317. NDT has converged: 1
  8318. Fitness score: 23.6006
  8319. Number of iteration: 10
  8320. (x,y,z,roll,pitch,yaw):
  8321. (0.067527, -0.129874, -0.0169084, -0.0154992, -0.0181769, 1.09923)
  8322. Transformation Matrix:
  8323. 0.454204 -0.890624 -0.022063 0.067527
  8324. 0.890712 0.454476 -0.00914952 -0.129874
  8325. 0.0181759 -0.0154961 0.999715 -0.0169084
  8326. 0 0 0 1
  8327. shift: 0.14638
  8328. -----------------------------------------------------------------
  8329. (Processed/Input): (438 / 438)
  8330. -----------------------------------------------------------------
  8331. Sequence number: 5368
  8332. Number of scan points: 7794 points.
  8333. Number of filtered scan points: 1117 points.
  8334. transformed_scan_ptr: 7794 points.
  8335. map: 6294 points.
  8336. NDT has converged: 1
  8337. Fitness score: 23.2523
  8338. Number of iteration: 6
  8339. (x,y,z,roll,pitch,yaw):
  8340. (-0.0363635, -0.153203, -0.0167384, -0.0141681, -0.0204996, 1.10955)
  8341. Transformation Matrix:
  8342. 0.444973 -0.895278 -0.0218092 -0.0363635
  8343. 0.895309 0.445282 -0.0120487 -0.153203
  8344. 0.0204982 -0.0141646 0.999689 -0.0167384
  8345. 0 0 0 1
  8346. shift: 0.15746
  8347. -----------------------------------------------------------------
  8348. (Processed/Input): (439 / 439)
  8349. -----------------------------------------------------------------
  8350. Sequence number: 5369
  8351. Number of scan points: 7776 points.
  8352. Number of filtered scan points: 1116 points.
  8353. transformed_scan_ptr: 7776 points.
  8354. map: 6294 points.
  8355. NDT has converged: 1
  8356. Fitness score: 23.2694
  8357. Number of iteration: 3
  8358. (x,y,z,roll,pitch,yaw):
  8359. (-0.121548, -0.143287, -0.0170433, -0.0134814, -0.0214885, 1.12053)
  8360. Transformation Matrix:
  8361. 0.435101 -0.900125 -0.0214876 -0.121548
  8362. 0.900125 0.435423 -0.0134767 -0.143287
  8363. 0.0214869 -0.0134779 0.999678 -0.0170433
  8364. 0 0 0 1
  8365. shift: 0.187897
  8366. -----------------------------------------------------------------
  8367. (Processed/Input): (440 / 440)
  8368. output_callback
  8369. filter_res: 0.2
  8370. filename: test_2.pcd
  8371. Original: 6294 points.
  8372. Filtered: 5131 points.
  8373. (Processed/Input): (441 / 440)
  8374. Saved 5131 data points to test_2.pcd.
RAW Paste Data

Adblocker detected! Please consider disabling it...

We've detected AdBlock Plus or some other adblocking software preventing Pastebin.com from fully loading.

We don't have any obnoxious sound, or popup ads, we actively block these annoying types of ads!

Please add Pastebin.com to your ad blocker whitelist or disable your adblocking software.

×