问题产生

        在采集rosbag时，设置的相机帧率为20fps，而TIM561激光雷达帧率仅有15Hz，从而导致通过这种方式导出的图片多于激光雷达数据，采用上一篇的染色方式，会导致时间戳的错位。

解决方式

        因为激光雷达和图片均采用时间戳+txt/png的方式进行命名，时间戳存储为小数点后6为，我的思路很简单，是让激光雷达和图片的时间戳进行做差求绝对值，设定一个阈值（我设定为0.05s），符合这个条件的再进行投影和染色。

代码

# -*- coding:utf-8 -*-
import cv2
import matplotlib.pyplot as plt
import math
import os
import numpy as np
import sys
from PIL import Image

#=========变量========#
laser_data_path = "你的激光雷达数据保存地址"
img_path = "你的图片保存地址"
rt_matrix = np.mat('-1.2010183413075470e-02 1.0256227843246418e-01 9.9465407782662085e-01 2.2413396467857991e-02;-9.9973138495993386e-01 -2.0950559598770235e-02 -9.9112046992586023e-03 -7.3982953946439764e-02;1.9822043801699996e-02 -9.9450593416793631e-01 1.0278634872532166e-01 4.7061647416975882e-02;0. 0. 0. 1.')
cam_matrix = np.mat('3.1990789776413413e+02 0. 3.1587721773926478e+02 0.;0. 3.1866536924385542e+02 2.3936008148761823e+02 0.;0. 0. 1. 0')
xs = []
ys = []
xyz_datas = []
j = 0
xyz_array = []
pix_x = []
pix_y = []
pix_z = []
pix_x1 = []
pix_y1 = []
RTX = []
RTY = []
RTZ = []
file_laser = sorted(os.listdir(laser_data_path))
file_img = sorted(os.listdir(img_path))
#=========变量========#




#=========角度========#
i = -135
angle_datas = []
while(1):
	if i <=135 :
		angle_datas.append(i)
		i = i +  0.3329223181
	else:
		break
#=========角度========#


mainColor=(42/256,87/256,141/256,1)
plt.xlabel("x",color=mainColor)
plt.ylabel("y",color=mainColor)
plt.tick_params(axis="x",colors=mainColor)
plt.tick_params(axis="y",colors=mainColor)


for laser_data1 in file_laser:
	laser_data_no_txt = laser_data1.replace('.txt','')
	h = 0 
	while(h != len(file_img)):
		for img_raw in file_img:
			img_raw_no_png = img_raw.replace('.png','')
			if abs(float(laser_data_no_txt)-float(img_raw_no_png)) < 0.05:
				print(laser_data_no_txt)
				print(img_raw_no_png)
				laser_data = os.path.join(laser_data_path,laser_data1)
				image = os.path.join(img_path,img_raw)
				img = cv2.imread(image)
				img2 = Image.open(image)
				img_array = img2.load()
				with open(laser_data, 'r') as f_laser:
					f_laser = f_laser.readlines()
					ranges = f_laser[13]
					ranges = ranges.replace("ranges: [","")
					ranges = ranges.replace(', ',",")
					ranges = ranges.replace(']\n',"")
					ranges = ranges.split(',')
					for range_data, angle_data in zip(ranges, angle_datas):
						angle_data = angle_data * (math.pi/180)
						x = float(range_data) * math.cos(angle_data)
						y = -(float(range_data) * math.sin(angle_data))
						xs.append(x)
						ys.append(y)
						xyz_datas.append(x)
						xyz_datas.append(y)
						xyz_datas.append(0)
						xyz_datas.append(1)
					while(j <= (len(xyz_datas)-4)):
						a = xyz_datas[j]
						b = xyz_datas[j+1]
						c = xyz_datas[j+2]
						d = xyz_datas[j+3]
						xyz_array.append(a)
						xyz_array.append(b)
						xyz_array.append(c)
						xyz_array.append(d)
						xyz_matrix = np.mat(xyz_array).T
						rt_matrix_convers = rt_matrix.I
						laser2camera_matrix = np.dot(rt_matrix_convers,xyz_matrix) #<======== 激光到相机
						j += 4
						RTX.append(laser2camera_matrix[0])
						RTY.append(laser2camera_matrix[1])
						RTZ.append(laser2camera_matrix[2])
						pix_matrix = np.dot(cam_matrix,laser2camera_matrix) #<======== 相机到图像
						pixx = (pix_matrix[0] / pix_matrix[2]) #u
						pixy = (pix_matrix[1] / pix_matrix[2]) #v
						pixz = pix_matrix[2] #s同时遍历全部文件
						xyz_array = []
						if  pixx > 0 and pixx <= 640 and pixy > 0 and pixy <= 480:
							pix_x.append(float(pixx))
							pix_y.append(float(pixy))
							pix_z.append(float(pixz))
				for pixx,pixy in zip(pix_x,pix_y):
					color = img_array[int(pixx),int(pixy)]
					red = color[0]/256
					green = color[1]/256
					blue = color[2]/256
					plt.plot(pixx,pixy,".",color = (red,green,blue))
					cv2.circle(img, (int(pixx),int(pixy)), 1, (0,0,255), 1)
				cv2.imshow(str(img_raw),img)
				plt.title(str(laser_data1))
				plt.show()
				cv2.destroyAllWindows()
				xs = []
				ys = []
				RTX = []
				RTY = []
				pix_x = []
				pix_y = []
				
		else:
			break