【学习笔记】Sick TIM561单线激光雷达点云上色

前提条件： 光达与相机已经联合标定完成。
染色的思路: 读取一帧的激光雷达点云数据，以及该帧点云对应的图像（点云数据以及图像数据均通过rosbag读取而得），将光达坐标系变换到图像坐标系。仅选取激光落在相机视场中的点，读取这些点对应的rgb值，然后绘制散点图时颜色使用读取到的rgb值；而将激光点显示在图像中使用了openCV在图像中绘制图形的方法。
整体代码如下：

import math
import cv2
import numpy as np
import matplotlib as mpl
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from PIL import Image

# range_datas是通过rosbag采集到的一帧的数据，sick TIM561雷达一帧数据811个点
range_datas = [1.8619999885559082, 1.8559999465942383, 1.8450000286102295, 1.8329999446868896, 1.8270000219345093, 0.10899999737739563, 0.09200000017881393, 0.08699999749660492, 0.06800000369548798, 0.061000000685453415, 0.05400000140070915, 0.05000000074505806, 0.050999999046325684, 0.050999999046325684, 0.05000000074505806, 0.0430000014603138, 0.04800000041723251, 0.050999999046325684, 0.05700000002980232, 0.054999999701976776, 0.05000000074505806, 0.05400000140070915, 0.04500000178813934, 0.04899999871850014, 0.057999998331069946, 0.06400000303983688, 0.06300000101327896, 0.06800000369548798, 0.06800000369548798, 0.07599999755620956, 0.06400000303983688, 0.06599999964237213, 0.07699999958276749, 0.07100000232458115, 0.05900000035762787, 0.057999998331069946, 0.041999999433755875, 0.0, 0.0, 0.0, 0.0020000000949949026, 0.0020000000949949026, 0.0020000000949949026, 1.2280000448226929, 1.2259999513626099, 1.2070000171661377, 1.187999963760376, 0.0020000000949949026, 0.0020000000949949026, 1.0959999561309814, 1.090000033378601, 1.0789999961853027, 1.0870000123977661, 1.0839999914169312, 1.0880000591278076, 1.1019999980926514, 1.1390000581741333, 1.1820000410079956, 1.2039999961853027, 1.2089999914169312, 1.2289999723434448, 1.2660000324249268, 1.281000018119812, 1.274999976158142, 1.2790000438690186, 1.2940000295639038, 1.2979999780654907, 1.2899999618530273, 1.3020000457763672, 1.3350000381469727, 1.3229999542236328, 1.2899999618530273, 1.2890000343322754, 1.2790000438690186, 1.1699999570846558, 1.0570000410079956, 1.034999966621399, 1.0390000343322754, 1.0889999866485596, 1.1030000448226929, 1.055999994277954, 1.0019999742507935, 1.0230000019073486, 1.0240000486373901, 1.0800000429153442, 1.1670000553131104, 1.1469999551773071, 1.0410000085830688, 1.0540000200271606, 1.0609999895095825, 0.9319999814033508, 0.8600000143051147, 0.8539999723434448, 0.8429999947547913, 0.8489999771118164, 0.9200000166893005, 1.0570000410079956, 1.2009999752044678, 1.1890000104904175, 1.0520000457763672, 0.9430000185966492, 0.9470000267028809, 0.8820000290870667, 0.8320000171661377, 0.800000011920929, 0.7799999713897705, 0.777999997138977, 0.7649999856948853, 0.7820000052452087, 0.777999997138977, 0.7670000195503235, 0.7459999918937683, 0.671999990940094, 0.6570000052452087, 0.6790000200271606, 0.8289999961853027, 0.0, 0.0, 0.0, 1.090000033378601, 1.0440000295639038, 1.0269999504089355, 1.0789999961853027, 1.1050000190734863, 1.059999942779541, 1.0160000324249268, 0.9900000095367432, 1.00600004196167, 1.062000036239624, 0.0, 0.0, 1.253000020980835, 0.0, 0.0, 0.0, 0.0, 0.0, 1.152999997138977, 0.0, 0.0, 1.25, 1.1699999570846558, 1.121000051498413, 1.1480000019073486, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.2940000295639038, 1.2940000295639038, 1.2940000295639038, 1.2890000343322754, 1.2960000038146973, 1.2890000343322754, 1.2929999828338623, 1.2899999618530273, 1.2920000553131104, 1.2879999876022339, 1.284999966621399, 1.2829999923706055, 1.2910000085830688, 1.2860000133514404, 1.2869999408721924, 1.2899999618530273, 1.284999966621399, 1.2869999408721924, 1.2920000553131104, 1.2890000343322754, 1.2920000553131104, 1.2920000553131104, 1.2949999570846558, 1.2949999570846558, 1.2960000038146973, 1.2979999780654907, 1.2970000505447388, 1.2929999828338623, 1.2990000247955322, 1.3009999990463257, 1.3040000200271606, 1.3070000410079956, 1.3040000200271606, 1.3109999895095825, 1.309999942779541, 1.312999963760376, 1.309999942779541, 1.315999984741211, 1.3179999589920044, 1.315999984741211, 1.319000005722046, 1.3179999589920044, 1.3170000314712524,