轮式里程计标定及二维激光数据去畸变

00.引言

二维激光数据结构查看rosmsg show sensor_msgs/LaserScan：

std_msgs/Header header	//　数据的消息头
  uint32 seq			//　数据的序号
  time stamp			//　数据的时间戳
  string frame_id		//　数据的坐标系
float32 angle_min		//　雷达数据的起始角度（最小角度）
float32 angle_max		//　雷达数据的终止角度（最大角度）
float32 angle_increment	//　雷达数据的角度分辨率（角度增量）
float32 time_increment	//　雷达数据每个数据点的时间间隔
float32 scan_time		//　当前帧数据与下一帧数据的时间间隔
float32 range_min		//　雷达数据的最小值
float32 range_max		//　雷达数据的最大值
float32[] ranges		//　雷达数据每个点对应的在极坐标系下的距离值
float32[] intensities	//　雷达数据每个点对应的强度值

实际数据距离：

header: 
  seq: 3568
  stamp: 
    secs: 1510031272
    nsecs: 522861940
  frame_id: "base_laser"
angle_min: -1.5708758831
angle_max: 1.5762219429
angle_increment: 0.00581718236208
time_increment: 6.17219993728e-05
scan_time: 0.0666666701436
range_min: 0.0500000007451
range_max: 25.0
ranges: [6.2769999504089355, 6.285999774932861, 6.296000003814697, 6.303999900817871, 6.315000057220459, 6.322999954223633, 6.313000202178955, 6.186999797821045, 6.116000175476074, 6.123000144958496, 6.13100004196167, 6.145999908447266, 6.1579999923706055, 6.163000106811523, 6.183000087738037, 6.193999767303467, 6.203000068664551, 6.2170000076293945, 6.234000205993652, 6.256999969482422, 6.306000232696533, 6.285999774932861, 6.290999889373779, 6.303999900817871, 6.318999767303467, 6.329999923706055, 6.3460001945495605, 6.359000205993652, 6.38100004196167, 6.394999980926514, 6.409999847412109, 6.428999900817871, 6.445000171661377, 6.460000038146973, 6.486000061035156, 6.494999885559082, 6.514999866485596, 6.5370001792907715, 6.546999931335449, 6.572999954223633, 6.590000152587891, 6.607999801635742, 6.636000156402588, 6.6529998779296875, 6.660999774932861, 10.97700023651123, 10.920000076293945, 11.164999961853027, 11.23799991607666, 0.0, 0.0, 0.0, 0.0, 14.932000160217285, 0.0, 0.0, 0.0, 13.340999603271484, 13.33899974822998, 13.338000297546387, 13.317999839782715, 13.29800033569336, 13.288000106811523, 13.262999534606934, 13.309000015258789, 0.0, 0.0, 0.0, 0.0, 0.0, 20.569000244140625, 20.45400047302246, 7.980000019073486, 7.979000091552734, 7.979000091552734, 7.986999988555908, 7.973999977111816, 7.9770002365112305, 7.9730000495910645, 7.9670000076293945, 7.960999965667725, 7.953000068664551, 7.936999797821045, 7.935999870300293, 0.0, 8.86400032043457, 0.0, 0.0, 14.166000366210938, 0.0, 0.0, 22.135000228881836, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 14.161999702453613, 2.2079999446868896, 2.2039999961853027, 2.193000078201294, 2.178999900817871, 2.1659998893737793, 2.1589999198913574, 2.1489999294281006, 2.13700008392334, 2.127000093460083, 2.1110000610351562, 2.1050000190734863, 2.0929999351501465, 2.0840001106262207, 2.072000026702881, 2.066999912261963, 2.0510001182556152, 2.055000066757202, 2.0409998893737793, 2.0299999713897705, 2.0209999084472656, 2.0250000953674316, 2.0280001163482666, 2.0390000343322754, 2.0199999809265137, 1.9900000095367432, 1.9759999513626099, 1.9630000591278076, 1.9609999656677246, 1.9570000171661377, 1.9509999752044678, 1.9420000314712524, 1.9329999685287476, 1.9299999475479126, 1.9229999780654907, 1.909000039100647, 1.9040000438690186, 1.8940000534057617, 1.8860000371932983, 1.8819999694824219, 1.8769999742507935, 1.8769999742507935, 1.8669999837875366, 1.8650000095367432, 1.8639999628067017, 1.8589999675750732, 1.8519999980926514, 1.840999960899353, 1.8569999933242798, 1.8849999904632568, 1.909000039100647, 1.9329999685287476, 1.9550000429153442, 1.9839999675750732, 2.010999917984009, 2.0329999923706055, 2.062999963760376, 2.0940001010894775, 2.121000051498413, 2.1510000228881836, 2.181999921798706, 2.2100000381469727, 2.246999979019165, 2.2799999713897705, 2.318000078201294, 2.3550000190734863, 2.4010000228881836, 2.424999952316284, 2.4630000591278076, 2.510999917984009, 2.5510001182556152, 2.5959999561309814, 2.6429998874664307, 2.687000036239624, 2.74399995803833, 2.7960000038146973, 2.8469998836517334, 2.9040000438690186, 2.996000051498413, 3.0910000801086426, 3.115999937057495, 3.130000114440918, 3.1429998874664307, 3.1659998893737793, 3.193000078201294, 9.24899959564209, 9.413999557495117, 7.545000076293945, 7.498000144958496, 7.423999786376953, 7.355000019073486, 7.2829999923706055, 7.2230000495910645, 7.1570000648498535, 7.10099983215332, 7.039999961853027, 6.9770002365112305, 6.928999900817871, 6.877999782562256, 6.883999824523926, 6.909999847412109, 6.936999797821045, 6.9679999351501465, 6.995999813079834, 7.03000020980835, 7.059999942779541, 7.0929999351501465, 7.127999782562256, 7.158999919891357, 7.198999881744385, 7.228000164031982, 7.263999938964844, 7.296000003814697, 7.336999893188477, 7.36899995803833, 7.400000095367432, 7.440999984741211, 7.480000019073486, 7.506999969482422, 11.678000450134277, 11.60200023651123, 11.538000106811523, 11.454999923706055, 11.381999969482422, 11.324999809265137, 11.255999565124512, 11.199000358581543, 11.234999656677246, 11.3149995803833, 11.388999938964844, 11.451000213623047, 11.508000373840332, 11.538000106811523, 13.14799976348877, 13.088000297546387, 13.005999565124512, 12.940999984741211, 12.871999740600586, 12.803999900817871, 12.746000289916992, 12.684000015258789, 12.619000434875488, 12.557000160217285, 12.512999534606934, 12.564000129699707, 12.654999732971191, 12.758000373840332, 12.829999923706055, 12.850000381469727, 14.444000244140625, 14.385000228881836, 14.32699966430664, 14.258000373840332, 14.199999809265137, 14.145999908447266, 14.09000015258789, 14.027000427246094, 13.968999862670898, 13.993000030517578, 14.125, 14.22700023651123, 14.29699993133545, 15.925000190734863, 15.859000205993652, 15.805999755859375, 15.73799991607666, 15.694999694824219, 15.666999816894531, 15.567000389099121, 15.541000366210938, 0.0, 0.0, 0.0, 12.972000122070312, 12.963000297546387, 12.939000129699707, 0.0, 12.852999687194824, 6.728000164031982, 6.696000099182129, 6.677000045776367, 6.659999847412109, 6.63700008392334, 6.614999771118164, 6.598999977111816, 6.578999996185303, 6.557000160217285, 6.541999816894531, 6.51800012588501, 6.506999969482422, 6.497000217437744, 6.520999908447266, 6.478000164031982, 6.433000087738037, 6.420000076293945, 6.408999919891357, 6