把激光点投影到图像上并融合显示

最新推荐文章于 2024-04-30 15:35:29 发布

weixin_44832383

最新推荐文章于 2024-04-30 15:35:29 发布

阅读量1.2k

点赞数 1

文章标签： python 计算机视觉开发语言

本文链接：https://blog.csdn.net/weixin_44832383/article/details/121095626

版权

把激光点投影到图像上并融合显示

示图：

在这里插入图片描述
全部代码如下（python）：

# -*- coding: utf-8 -*-
import pcl
import numpy as np
import cv2 as cv
from PIL import Image
from pylab import imshow
from pylab import array
from pylab import plot
from pylab import title
from pylab import *
import matplotlib.pyplot as plt

x=[]
y=[]
distance=[]    #存放需要投影点转换成二维前的雷达坐标的x坐标（距离信息），以此为依据对投影点进行染色。
distance_3d=[]    #存放转换前雷达坐标的x坐标（距离信息）。



cloud = pcl.load('E:\\shu_ju\\selection\\2021-03-03-15-17-24\\pcd_1\\0_1614755848 - Cloud.pcd')
im = Image.open('E:\\shu_ju\\selection\\2021-03-03-15-17-24\\image2\\1614755848.902324.png')

pix = im.load()
points_3d = []
# print('Loaded ' + str(cloud.width * cloud.height) +
#       ' data points from test_pcd.pcd with the following fields: ')
for i in range(0, cloud.size):
    x_raw = float(cloud[i][0])
    y_raw = float(cloud[i][1])
    z_raw = float(cloud[i][2])
    point_3d = []

    point_3d.append(x_raw)
    point_3d.append(y_raw)
    point_3d.append(z_raw)
    
    if y_raw>0:
        points_3d.append(point_3d)
        distance_3d.append(y_raw)
    '''    
    points_3d.append(point_3d)
    distance_3d.append(y_raw)'''

cube = np.float64(points_3d)
#print(cube.shape)


rvec = np.float64([1.680647483853886, 0.03782614262625567, 0.003707885488685594])  #旋转向量
tvec = np.float64([-0.3270823619145787, 1.994427053985835, -0.2688515838179673])  #平移向量

camera_matrix = np.float64([[1400.660521866737, 0, 989.6663020916587],
                            [0, 1397.477295771064, 594.9904177802305],
                            [0, 0, 1]])  #内参矩阵

distCoeffs = np.float64([-0.05694747808562394, -0.08329212973455258,
                         -0.0009314071183112955, 0.006712153417379347, 0.2493801178842133])  #畸变参数，[k1,k2,p1,p2,k3]


point_2d, _ = cv.projectPoints(cube, rvec, tvec, camera_matrix, distCoeffs)
#print(point_2d[0].shape)
point_2d_sque = np.squeeze(point_2d)
print(point_2d_sque)
np.savetxt('E:\\shu_ju\\selection\\2021-03-03-15-17-24\\pcd_1\\0_1614755848 - Cloud.txt', point_2d_sque)
m=-1
for point in point_2d:
    m=m+1
    x_2d = point[0][0]
    y_2d = point[0][1]

    if 0<=x_2d<1920 and 0<=y_2d<1200:
        x.append(x_2d)
        y.append(y_2d)
        distance.append(-distance_3d[m]*100)#数值取反是为了让colormap颜色由红到蓝显示而非由蓝到红
        RGB=pix[x_2d,y_2d]
        #print('x,y,z,(r,g,b):',([x_2d,y_2d,distance_3d[m]],RGB))


x=np.array(x)
y=np.array(y)
plt.scatter(x, y, c=distance, cmap='jet',s=1,marker='.')
plt.imshow(im)
plt.show()