大致思路

感兴趣区域roi设置

映射点到像素上

移动坐标原点

根据高度值填充像素值

环境配置conda+ros

因为ros中的包是依赖Python2的，但是我们想用Python3b编程，所以需要安装一个conda环境。

在.bashrc中修改默认的conda环境变量。

# Conda

# export PATH=/home/s/anaconda3/bin:$PATH # 注释掉conda的环境变量

alias condapy3='. /home/s/anaconda3/etc/profile.d/conda.sh && conda activate py3.7' # 创建Python3.7的环境

打开终端默认还是python2，输入condapy3才进入python环境。

在py3.7环境中安装包

pip install catkin-tools

pip install rospkg

这样就可以通过虚拟环境编译script文件了

以这个package为例，使用

catkin_make

，然后

rosrun script文件时，要在conda环境下，这样使用的一些package才是指向该conda环境中的package的对应文件。

我们这个conda环境中的opencv版本如下

所以在执行script文件时打印出来是对应的版本：

(py3.7) ➜ catkin_ws clear

(py3.7) ➜ catkin_ws echo $ROS_PACKAGE_PATH

/home/s/catkin_ws/src:/home/s/ros_study/src:/opt/ros/kinetic/share

(py3.7) ➜ catkin_ws rosrun aiimooc_syz2 aiimooc_syz2.py

opencv: 4.2.0

open3d:0.7.0.0

ros实现(Python)

因为cv2和ros自带的cv有矛盾，我们想使用conda中自己安装的cv2，所以在import之前，先把ros中路径移除，在import之后在导入。

bird_eyes.py

#!/usr/bin/env python3

# use python3 in current conda env!!!

# import cv2 in conda rather than in ros

import sys

sys.path.remove('/opt/ros/kinetic/lib/python2.7/dist-packages')

import cv2

# 使用完cv2后再导入ros路径

sys.path.append('/opt/ros/kinetic/lib/python2.7/dist-packages')

from cv_bridge import CvBridge

import numpy as np

import rospy

from sensor_msgs.msg import PointCloud2,Image

import sensor_msgs.point_cloud2 as pc2

import open3d

import os

class pt2brid_eye:

def __init__(self):

self.image_pub = rospy.Publisher('/bird_eyes', Image, queue_size=10)

self.pt_sub=rospy.Subscriber("/rslidar_points", PointCloud2, self.callback)

self.bridge = CvBridge()

def callback(self,lidar):

lidar = pc2.read_points(lidar)

points = np.array(list(lidar))

im = self.point_cloud_2_birdseye(points) # im is a numpy array

self.image_pub.publish(self.bridge.cv2_to_imgmsg(im))

print("convert!")

def point_cloud_2_birdseye(self,points):

x_points = points[:, 0]

y_points = points[:, 1]

z_points = points[:, 2]

f_filt = np.logical_and((x_points > -50), (x_points < 50))

s_filt = np.logical_and((y_points > -50), (y_points < 50))

filter = np.logical_and(f_filt, s_filt)

indices = np.argwhere(filter)

x_points = x_points[indices]

y_points = y_points[indices]

z_points = z_points[indices]

x_img = (-y_points*10).astype(np.int32)+500

y_img = (-x_points *10).astype(np.int32)+500

pixel_values = np.clip(z_points,-2,2)

pixel_values = ((pixel_values +2) / 4.0) * 255

im=np.zeros([1001,1001],dtype=np.uint8)

im[y_img, x_img] = pixel_values

return im

# def cloud_subscribe():

# rospy.init_node('cloud_subscribe_node')

# pub = rospy.Publisher('/bird_eyes', Image, queue_size=10)

# rospy.Subscriber("/rslidar_points", PointCloud2, callback)

# rospy.spin()

if __name__ == '__main__':

print("opencv: {}".format(cv2.__version__))

print("open3d:{}".format(open3d.__version__))

# cloud_subscribe()

pt2img=pt2brid_eye()

rospy.init_node('pt_to_brid_eyeImage_node')

rospy.spin()

可视化

完整代码

https://github.com/suyunzzz/aiimooc_lesson/tree/master/aiimooc_syz2

参考

http://ronny.rest/tutorials/module/pointclouds_01/point_cloud_birdseye/

http://ronny.rest/blog/post_2017_03_26_lidar_birds_eye/