0. 前言
前段时间去大概了解了如何去滤除动态障碍物的问题,也根据调研去做了一些工作,这一篇文章主要向大家展示如何将Lidar-MOS和ROS2结合起来去使用。
1. 环境安装
文中使用了Salsanext,Rangenet ++和Mine三个模块作为baseline来设计和测试动态障碍物滤除的工作,其中的语义分割工作都是目前已有的,可以去原项目中查看。
代码下载:
#下载程序
git clone https://github.com/PRBonn/LiDAR-MOS.git
cd data
wget https://www.ipb.uni-bonn.de/html/projects/LiDAR-MOS/LiDAR_MOS_toy_dataset.zip
unzip LiDAR_MOS_toy_dataset
conda create --name lidar-mos python=3.7
conda activate lidar-mos
pip -r requirements.txt -i https://pypi.tuna.tsinghua.edu.cn/simple
注意,源代码之前的适配版本是cuda9和cuda10,且版本过旧,这里提供一个新版的requirements.txt
absl-py==0.8.0
astor==0.8.0
cycler==0.10.0
grpcio==1.25.0
h5py==2.10.0
kiwisolver==1.1.0
matplotlib==2.2.3
mock==3.0.5
opencv-contrib-python==4.1.0.25
opencv-python==4.1.0.25
pillow==6.1.0
protobuf==3.10.0
pyparsing==2.4.2
python-dateutil==2.8.0
pytz==2019.2
pyyaml==5.1.1
tensorboard==2.1.0
tensorflow==2.1.0
tensorflow-gpu==2.1.0
tensorflow-estimator==2.1.0
termcolor==1.1.0
torch==1.6.0
torchvision==0.7.0
PyQt5
pyqt5-tools
tqdm
2. 使用SalsaNext作为baseline
2.1 生成残差图像
要使用Lidar-mos的方法,生成残差图像:
$ python3 utils/gen_residual_images.py
2.2 推断数据
首先我们会拿取kitti数据集文件,并下载预验证的模型来生成LiDAR-MOS预测(download)。
$ cd mos_SalsaNext/train/tasks/semantic
$ python3 infer.py -d ../../../../data -m ../../../../data/model_salsanext_residual_1 -l ../../../../data/predictions_salsanext_residual_1_new -s valid
2.3 训练数据
要从零开始使用SalsaNext训练激光雷达MOS网络,必须下载KITTI-Odometry dataset和Semantic-Kitti dataset:
$ cd mos_SalsaNext
$ ./train.sh -d ../data -a salsanext_mos.yml -l path/log -c 0 # the number of used gpu cores
评估和可视化
How to evaluate
评估指标。让我们将移动(动态)状态称为D,将静态状态称为s。
由于我们忽略了未标记和无效状态,因此在MOD中只有两个类。
GT\Prediction | dynamic | static |
---|---|---|
dynamic | TD | FS |
static | FD | TS |
- $$ IoU_{MOS} = \frac{TD}{TD+FD+FS} $$
要评估LiDAR_MOS_toy_dataset上的MOS结果,只需运行
To evaluate the MOS results on the toy dataset just run:
$ python3 utils/evaluate_mos.py -d data -p data/predictions_salsanext_residual_1_valid -s valid
要在我们的激光雷达MOS基准上评估MOS结果,请查看我们的semantic kitti api和基准网站.
How to visualize the predictions
要在LiDAR_MOS_toy_dataset数据集上可视化MOS结果,只需运行
$ python3 utils/visualize_mos.py -d data -p data/predictions_salsanext_residual_1_new -s 8 # here we use a specific sequence number
-
‘sequence’是要访问的序列。
-
‘dataset’是序列目录所在的kitti数据集的路径。
-
“n”是下一次扫描,
-
“b”是上一次扫描,
-
‘esc’或’q’退出。
下面是运行出来的结果☺
3. Lidar-MOS与ROS2
这里作者先写了一个初稿,基本含义是将PointCloud2转为bin文件数据格式,并传入Lidar-MOS,然后再将Lidar-MOS的输出结果拿出来,用PointCloud2格式发布出来,目前还没完全写完,后面有时间再写吧
import rclpy
from rclpy.node import Node
import numpy as np
import os
import sys
import time
from sensor_msgs.msg import PointCloud2
from sensor_msgs.msg import PointField
from std_msgs.msg import String
import copy
from segment.point_cloud2 import read_points,create_cloud
class SegmentDeynamicObject(Node):
def __init__(self,name):
super().__init__(name)
self.get_logger().info('segment_deynamic_object')
self.pub_ = self.create_publisher(PointCloud2, '/segment_pub', 10)
self.sub_ = self.create_subscription(PointCloud2, '/segment_sub', self.pointcloud_callback, 10)
self.queue_ = []
self.time_stamp_queue_=[]
self.deep_queue_ = []
self.bin_path_ = ""
#self.timer1 = self.create_timer(5,bag2bin)
bag2bin()
# lidar-mos中间处理
bin2bag()
def pointcloud_callback(self, msg):
self.queue_.append(msg)
self.time_stamp_queue_.append(msg.header)
def bag2bin(self):
if len(self.queue_) == 0:
pass
point_clouds = self.queue_[0]
self.queue_.pop(0)
# save to bin file
bin_list = []
gen = read_points(point_clouds,field_names=("x","y","z"), skip_nans=True)
for g in gen:
bin_list.append(g[0],g[1],g[2],g[3])
np_pc = np.array(bin_list,dtype=np.float32)
np_pc_bin = copy.deepcopy(np_pc)
# self.bin_path_ = os.path.join(os.path.dirname(os.path.abspath(__file__)),'bin')
# np_pc_bin.astype(np.float32).tofile(bin_path_)
self.deep_queue_.append(np_pc_bin)
return np_pc_bin
def bin2bag(self):
if len(self.deep_queue_) == 0:
pass
deep_learning_clouds = self.deep_queue_[0]
self.deep_queue_.pop(0)
cloud_np = deep_learning_clouds.reshape([-1, 4])
# cloud_np = np.fromfile(bin_path_,dtype=np.float32,count=-1).reshape([-1, 4])
size = np.size(cloud_np, 0)
cloud = []
for i in range(size):
point = [cloud_np[i, 0], cloud_np[i, 1],
cloud_np[i, 2], cloud_np[i, 3]]
cloud.append(point)
fields = [PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1),
PointField('intensity', 12, PointField.FLOAT32, 1)]
msg_point_cloud = create_cloud(header=self.time_stamp_queue_[0],fields=fields,points=cloud)
self.time_stamp_queue_.pop(0)
self.pub_.publish(msg_point_cloud)
def main(args=None):
rclpy.init()
node = SegmentDeynamicObject('segment_deynamic_object')
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
评论(3)
您还未登录,请登录后发表或查看评论