你的ROS+PointPillars推理酱紫快！？

       在成功训练了自定义的PointPillars模型后，接下来就涉及到如何把这个模型使用起来了。笔者参考了如下几篇文章：

---

       ROS点云的Pointpillars实时目标检测_ros 点云_橘子皮一下的博客-CSDN博客

       基于openpcdet的Pointpillars之ROS可视化_openpcdet复现_PX Yue的博客-CSDN博客

---

        虽然参考文章中都将ROS和PointPillars成功结合了起来，但是还存在因为模型推理的速度太慢导致检测框跟不上点云变化的问题，最直观的体现就是在rviz中检测结果会产生明显的漂移，显然无法达到我们要进行实时目标检测的目的。

        因此，下文通过分析各步骤耗时并通过Cython解决部分步骤耗时长的问题。

文章目录

• 前言
• 一、代码及其耗时分析
• 二、Cython是什么
• 三、如何使用
• 四、效果验证

---

前言

        对于我之前基于OpenPCDet框架训练得到的PointPillars模型，笔者本想使用TensorRT进行加速，但由于在复用github上找到的项目时，发现经过TensorRT加速后的检测效果相较于直接使用pth模型进行demo推理的效果差很多，因此需要另找方法对该实时demo推理进行加速，但在这之前需要对代码以及其总耗时由哪些部分组成及进行分析。

---

一、代码及其耗时分析

        下面的代码是基于上面参考的文章所述进行修改，仅供参考，可结合自身具体需求继续修改。(下文中凡是带"xxx""xxxxxx"字样的地方都代表是需要根据自己的具体使用情况进行修改)

#!/usr/bin/env python3

import rospy
from sensor_msgs.msg import PointCloud2
import sensor_msgs.point_cloud2 as pc2
from std_msgs.msg import Header
from jsk_recognition_msgs.msg import BoundingBox, BoundingBoxArray
import sys

# 笔者自己的路径
sys.path.append("/home/xxx")
# /opt/ros/noetic/lib/python3/dist-packages/jsk_recognition_msgs/msg/_BoundingBox.py
sys.path.append("/opt/ros/noetic/lib/python3/dist-packages")


import time
import numpy as np
from pyquaternion import Quaternion

import argparse
import glob
from pathlib import Path

# import mayavi.mlab as mlab
import numpy as np
#import cupy
import torch
import scipy.linalg as linalg
import ctypes

# 这是笔者自己电脑上生成的cython库路径，修改成自己对应的路径即可。
lib = ctypes.CDLL("/home/xxx/pointpillars_ros/tools/receive_pointcloud.cpython-38-aarch64-linux-gnu.so")

import receive_pointcloud
import msg2list


from pcdet.config import cfg, cfg_from_yaml_file
from pcdet.datasets import DatasetTemplate
from pcdet.models import build_network, load_data_to_gpu
from pcdet.utils import common_utils

class DemoDataset(DatasetTemplate):
    def __init__(self, dataset_cfg, class_names, training=True, root_path=None, logger=None, ext='.bin'):
        """
        Args:
            root_path: 根目录
            dataset_cfg: 数据集配置
            class_names: 类别名称
            training: 训练模式
            logger: 日志
            ext: 扩展名
        """
        super().__init__(
            dataset_cfg=dataset_cfg, class_names=class_names, training=training, root_path=root_path, logger=logger
        )
        


class Pointpillars_ROS:
    def __init__(self):
        config_path, ckpt_path = self.init_ros()
        self.init_pointpillars(config_path, ckpt_path)




    def init_ros(self):
        """ Initialize ros parameters """
        config_path = rospy.get_param("/config_path", "/home/xxx//src/pointpillars_ros/tools/cfgs/custom_models/pointpillar.yaml")
        ckpt_path = rospy.get_param("/ckpt_path", "/home/xxx/OpenPCDet/output/checkpoint_epoch_199.pth")
        self.sub_velo = rospy.Subscriber("/rslidar_points", PointCloud2, self.lidar_callback, queue_size=1,  buff_size=2**12)
        self.pub_bbox = rospy.Publisher("/pointpillars_ros_visual", BoundingBoxArray, queue_size=1)
        self.pub = rospy.Publisher("/xxxxxx", msg_type, queue_size=1)
        return config_path, ckpt_path


    def init_pointpillars(self, config_path, ckpt_path):
        """ Initialize second model """
        logger = common_utils.create_logger() # 创建日志
        logger.info('-----------------Quick Demo of Pointpillars-------------------------')
        cfg_from_yaml_file(config_path, cfg)  # 加载配置文件
        
        self.demo_dataset = DemoDataset(
            dataset_cfg=cfg.DATA_CONFIG, class_names=cfg.CLASS_NAMES, training=False,
            ext='.bin', logger=logger
        )
        self.model = build_network(model_cfg=cfg.MODEL, num_class=len(cfg.CLASS_NAMES), dataset=self.demo_dataset)
        # 加载权重文件
        self.model.load_params_from_file(filename=ckpt_path, logger=logger, to_cpu=True)
        self.model.cuda() # 将网络放到GPU上
        self.model.eval() # 开启评估模式


    def rotate_mat(self, axis, radian):
        rot_matrix = linalg.expm(np.cross(np.eye(3), axis / linalg.norm(axis) * radian))
        return rot_matrix

    # import ctypes
    # lib = ctypes.CDLL("/home/xxx/pointpillars_ros/tools/receive_pointcloud.cpython-38-aarch64-linux-gnu.so")

    def lidar_callback(self, msg):
        """ Captures pointcloud data and feed into second model for inference """
        rostime_start = rospy.Time.now()

        # rostime_receive_front = rospy.Time.now()
        pcl_msg = pc2.read_points(msg, skip_nans=True, field_names=("x", "y", "z","intensity"))
        # print('====print what type pcl_msg is: ',type(pcl_msg))
        # rostime_read_points_rear = rospy.Time.now()
        # rospy.loginfo("本次生成pcl_msg用时：%.6f ms" ,(rostime_read_points_rear.to_sec()-rostime_receive_front.to_sec())*1000)


        # rostime_list_front = rospy.Time.now()
        # my_list = list(pcl_msg)
        # my_list = msg2list.generate_list(pcl_msg)
        my_list = [point for point in pcl_msg]
        # print('===print my_list shape',len(my_list),my_list[0])
        #print("-------------",my_list[0])
        #my_new_list = [float(x) for x in my_list]
        # rostime_list_rear = rospy.Time.now()
        # rospy.loginfo("本次生成list用时：%.6f ms" ,(rostime_list_rear.to_sec()-rostime_list_front.to_sec())*1000)

        # rostime_array_front = rospy.Time.now()
        np_p = receive_pointcloud.to_array(my_list)
        #np_p = np.array(my_list)
        # rostime_array_rear = rospy.Time.now()
        # rospy.loginfo("本次list转array用时：%.6f ms" ,(rostime_array_rear.to_sec()-rostime_array_front.to_sec())*1000)


        # rostime_receive_rear = rospy.Time.now()
        # rospy.loginfo("本次接收点云用时：%.6f ms" ,(rostime_receive_rear.to_sec()-rostime_receive_front.to_sec())*1000)

        # 笔者此处不需要像原作者一样进行点云坐标变换，所以将点云点乘旋转矩阵这一步注释，改为直接赋值，这一步耗时也不短，根据设备不同可能会有所差异。
        # np_p = np.fromiter(pcl_msg,dtype=np.float32)
        # 旋转轴
        # rand_axis = [0,1,0]
        #旋转角度0.1047
        # yaw = 0
        #返回旋转矩阵
        # rot_matrix = self.rotate_mat(rand_axis, yaw)
        # np_p_rot = np.dot(rot_matrix, np_p[:,:3].T).T
        
        # convert to xyzi point cloud
        # x = np_p_rot[:, 0].reshape(-1)
        # y = np_p_rot[:, 1].reshape(-1)
        # z = np_p_rot[:, 2].reshape(-1)
        # x = np_p[:, 0].reshape(-1)
        # y = np_p[:, 1].reshape(-1)
        # z = np_p[:, 2].reshape(-1)
        # print("array type is ",type(np_p))
        # print("array[0] is ",np_p[0])
        x = np_p[:, 0].reshape(-1)
        #print('=========print x shape=======')
        y = np_p[:, 1].reshape(-1)
        z = np_p[:, 2].reshape(-1)
        if np_p.shape[1] == 4: # if intensity field exists
            i = np_p[:, 3].reshape(-1)
        else:
            i = np.zeros((np_p.shape[0], 1)).reshape(-1)
        points = np.stack((x, y, z, i)).T

        # print(points.shape)
        # 组装数组字典
        input_dict = {
            'points': points,
            'frame_id': msg.header.frame_id,
        }
        # rostime_prepare_data_front = rospy.Time.now()
        data_dict = self.demo_dataset.prepare_data(data_dict=input_dict) # 数据预处理
        # rostime_prepare_data_rear = rospy.Time.now()
        # rospy.loginfo("本次prepare用时：%.6f ms" ,(rostime_prepare_data_rear.to_sec()-rostime_prepare_data_front.to_sec())*1000)

        
        # rostime_data_dict_front = rospy.Time.now()
        data_dict = self.demo_dataset.collate_batch([data_dict])
        # rostime_data_dict_rear = rospy.Time.now()
        # rospy.loginfo("本次data_dict用时：%.6f ms" ,(rostime_data_dict_rear.to_sec()-rostime_data_dict_front.to_sec())*1000)

        
        # rostime_load_data2gpu_front = rospy.Time.now()
        load_data_to_gpu(data_dict) # 将数据放到GPU上
        # rostime_load_data2gpu_rear = rospy.Time.now()
        # rospy.loginfo("本次data2gpu用时：%.6f ms" ,(rostime_load_data2gpu_rear.to_sec()-rostime_load_data2gpu_front.to_sec())*1000)

        
        # rostime_forward_front = rospy.Time.now()
        pred_dicts, _ = self.model.forward(data_dict) # 模型前向传播
        # rostime_forward_rear = rospy.Time.now()
        # rospy.loginfo("本次infer_forward用时：%.6f ms" ,(rostime_forward_rear.to_sec()-rostime_forward_front.to_sec())*1000)

        # rostime_gpu2cpu_front = rospy.Time.now()
        # boxes_lidar返回的是预测的bbox，（num_detections,7）7-> x y z l w h raw
        boxes_lidar = pred_dicts[0]['pred_boxes'].detach().cpu().numpy()
        scores = pred_dicts[0]['pred_scores'].detach().cpu().numpy()
        label = pred_dicts[0]['pred_labels'].detach().cpu().numpy()
        num_detections = boxes_lidar.shape[0]
        # rostime_gpu2cpu_rear = rospy.Time.now()
        # rospy.loginfo("本次gpu2cpus用时：%.6f ms" ,(rostime_gpu2cpu_rear.to_sec()-rostime_gpu2cpu_front.to_sec())*1000)

        xxxxxx = xxxxxx()
        # print('=====boxes lidar========:\n',type(boxes_lidar),'\n',boxes_lidar.shape)
        # print('=====print boxes_lidar x y z ',type(boxes_lidar[1]),boxes_lidar[1].shape)
        for i in range(num_detections):
            xxx = xxx()
            xxx.poseConfidence.data = scores[i]
            xxx.position.x = float(boxes_lidar[i][0])
            xxx.position.y = float(boxes_lidar[i][1])
            xxx.position.z = float(boxes_lidar[i][2])
            xxx.xxxxxx.append(xxx)
        xxx.header.frame_id = msg.header.frame_id
        xxx.header.stamp = rospy.Time.now()
        self.pub.publish(xxx)

        rospy.loginfo("The num is: %d ", num_detections)

        rostime_end = rospy.Time.now()
        rospy.loginfo("本次推理总用时：%.6f ms" ,(rostime_end.to_sec()-rostime_start.to_sec())*1000)

        # print(boxes_lidar)
        # print('=====print scores===========',type(scores),scores.shape)
        # print(label)


        # 使用rviz进行可视化
        arr_bbox = BoundingBoxArray()
        for i in range(num_detections):
            bbox = BoundingBox()

            bbox.header.frame_id = msg.header.frame_id
            bbox.header.stamp = rospy.Time.now()
            bbox.pose.position.x = float(boxes_lidar[i][0])
            bbox.pose.position.y = float(boxes_lidar[i][1])
            bbox.pose.position.z = float(boxes_lidar[i][2]) + float(boxes_lidar[i][5]) / 2
            bbox.dimensions.x = float(boxes_lidar[i][3])  # width
            bbox.dimensions.y = float(boxes_lidar[i][4])  # length
            bbox.dimensions.z = float(boxes_lidar[i][5])  # height
            q = Quaternion(axis=(0, 0, 1), radians=float(boxes_lidar[i][6]))
            bbox.pose.orientation.x = q.x
            bbox.pose.orientation.y = q.y
            bbox.pose.orientation.z = q.z
            bbox.pose.orientation.w = q.w
            bbox.value = scores[i]
            bbox.label = label[i]

            arr_bbox.boxes.append(bbox)
        
        arr_bbox.header.frame_id = msg.header.frame_id
        arr_bbox.header.stamp = rospy.Time.now()
        
        self.pub_bbox.publish(arr_bbox)


if __name__ == '__main__':
    sec = Pointpillars_ROS()
    rospy.init_node('pointpillars_ros_node', anonymous=True)
    try:
        rate = rospy.Rate(15)
        while not rospy.is_shutdown():
            rate.sleep()
            
        
    except KeyboardInterrupt:
        del sec
        print("Shutting down")

        结合上述代码，笔者对在callback回调函数中的每一步操作都进行了计时（使用ROS自带的计时标志，但后续发现计时设置太多也会影响整体耗时），分析如下：

   

        由于笔者的激光雷达扫描频率设置为10HZ，也就是100ms/帧，如果目标检测中一帧的处理时间接近100ms甚至是大于100ms，这非常不利于感知模块后诸如规划、控制到整车执行等的正常运行，而且还要尽可能的为一些通信延迟提供余量，这也即是说激光雷达目标检测这一步理论上耗时越短越好，就可以为后续的模块腾出更多时间余量。

        为了减少一帧推理的总用时，笔者决定从减少存储点云信息的列表list转为numpy的数组array形式这一步用时入手，原因主要有如下两个：

（1）从上表中我们得知，回调函数中最耗时的是将点云信息存储到列表list以及该list转为numpy的数组array形式这一步耗时很长，几乎占了整体用时的一半以上；

（2）在回调函数中的所有必要步骤中，这一步骤相对来说容易减少耗时。（因为像有些步骤本来已经不算很耗时、有些像模型inference耗时虽然比较长，但是笔者水平较为有限，还不知道该怎么去减少这一步的时长，手动狗头>_<）

二、Cython是什么

1.简介

         Cython是让Python脚本支持C语言扩展的编译器，Cython能够将Python+C混合编码的.pyx脚本转换为编译性的C代码，主要用于优化Python脚本性能或者Python调用C函数库。

        笔者个人的一点理解是：从常用的C语言和Python语言属性出发，C语言是编译性语言，Python是解释性语言，通常来说在性能表现上两者各有优缺，C语言优点是编译后运行速度快，但缺点是代码需要编译后才能执行且有些编译耗时较长；Python语言优点是不需要编译、书写方便，但是执行速度相对较慢。

        举个例子，对于C语言，变量需要在书写代码时就指定类型，这样在编译后运行就不需要再次解析该变量类型，进而在一定程度上加快了运行速度；对于Python语言，变量在书写代码时无需指定类型，书写方便，但是在执行时需要解释器对变量进行分析得到该变量的类型，进而在一定程度上拖慢了运行速度。

        而采用Cython，就有点类似于在Python中使用C的特性，只需要在某些需要高性能的步骤上使用，既可以在此处利用C语言运行速度快的优点，又能在整体代码上不失Python语言的简洁方便。

2.参考资料

​​​​​​【深度好文】python加速库cython简介_python cython_赵卓不凡的博客-CSDN博客

Cython+NumPy加速法，让你的Python加速百倍_哔哩哔哩_bilibili

Cython: C-Extensions for Python

Cython教程与代码之——Cython_tutorial之（1到4） - 知乎 (zhihu.com)

三、如何使用

        Cython的使用见仁见智，可以根据自己所需上网进行相应检索，下面仅为笔者所用和一些简单的使用介绍，基本上可划分为如下步骤：

        * 编写.pyx文件

        * 编写setup.py脚本去调用Cython编译器将.pyx文件编译为.c文件

        * C编译器会把.c文件编译成.so文件或者是.pyd文件，取决于是何种操作系统，前者是                       Linux、后者是Windows

        * 在代码中链接.so动态链接库文件，并调用具体方法实现具体功能

        对上述几个文件名后缀，可能没有接触过的友友们看到会有些好奇，笔者这里做一个简单的介绍：

        * pyx文件是Cython编程语言的源代码文件的扩展名，其地位相当于cpp于C/C++、py于Python。而一个pyx文件若想要被python语言直接使用，需要先被编译成.c文件，再编译成.pyd（Windows）或.so（Linux）文件，才可以在python中当成时module使用，如：“import xxx”。

        * 可能有的友友对为什么一个pyx文件要经过两次编译，从.pyx变成.c，再从.c变成.so才能被使用呢感到疑惑，笔者这里通过分析两次编译各自的作用来解释一下，有兴趣的友友可以自行查找相关资料深入了解：

        （1）.pyx文件包含了Python和C的混合代码，通过将.pyx文件编译成C文件，可以将Python代码转换为C代码，以获得更高的执行速度、更好的性能以及与C语言及其库的操作性（因为C代码可以方便地与C库进行互操作，而Python也提供了许多与C库交互的工具和接口。通过将Python代码编译成C代码，可以更轻松地与现有的C库进行集成，提供更广泛的功能。）

        （2）C文件进一步编译为.so共享对象文件时，有助于实现动态链接、模块化设计、跨平台兼容性、隐藏实现细节等，但对于笔者这样的小白来说，可能更看重的是能够动态链接，.so文件是一种可被动态链接的二进制文件，可以在运行时动态加载到内存中，这意味着我们可以在不重新编译代码的情况下多次使用该文件，并将其与其他代码动态链接到一起，灵活性和复用性高。

---

        下面来介绍一下具体的使用过程，首先可通过pip来安装Cython：

pip install Cython

        安装好后，确定自己的需求，然后进行.pyx文件的书写（以笔者上述所要实现的python中list转成array为例）：

cimport cython
import numpy as np

cpdef to_array(list inp):
    cdef float[:,:] arr = np.zeros((len(inp),4),dtype=np.float32)
    cdef Py_ssize_t idx
    for idx in range(len(inp)):
        for i in range(4):
            arr[idx][i] = inp[idx][i]
    return np.asarray(arr)

        上述代码为笔者所进行的list转成array操作，定义了一个cython函数cpdef to_array，输入为list类型，输出为array类型，届时在代码中进行调用该函数即可。

        编写完.pyx文件后，需要使用编译器将其编译为c文件，创建一个setup.py文件（名字无所谓），编写如下，基本只需要指定一下自己的.pyx文件路径就好。

from setuptools import setup
from Cython.Build import cythonize

setup(
    ext_modules=cythonize("/home/xxx/pointpillars_ros/tools/receive_pointcloud.pyx")
)

        编写完setup.py文件后，可通过在终端执行如下命令完成将.pyx文件编译为.c文件和.so文件：

python setup.py build_ext --inplace

        执行完后，可在当前文件夹下看到共有如下3个文件存在（笔者所用环境为Linux）： 

 

         

---

        完成上述步骤后，意味着已经把这个具有实现具体功能的.so写好了，接下来就是要用在我们的代码里了，这也不难，就是像别的库一样import后使用就好，belike👇：

import ctypes

'''
在Python中，.so文件可以通过ctypes模块加载，使用ctypes.CDLL函数加载一个.so文件时，
可以通过函数名和参数列表来调用其中的函数，也就是可以在Python中调用C代码提供的功能
'''

lib =
ctypes.CDLL("/home/scutracing/pp_ros/src/pointpillars_ros/tools/receive_pointcloud.cpython-38-aarch64-linux-gnu.so")

import receive_pointcloud


def lidar_callback(self, msg):
    pcl_msg = pc2.read_points(msg, skip_nans=True, field_names=("x", "y", "z","intensity"))
    my_list = [point for point in pcl_msg]

    np_p = receive_pointcloud.to_array(my_list)
        
    x = np_p[:, 0].reshape(-1)
    y = np_p[:, 1].reshape(-1)
    z = np_p[:, 2].reshape(-1)

         显然，我们在np_p = receive_pointcloud.to_array(my_list)这一句中已经成功调用了我们先前编写好的Cython代码，那接下来就验证一下改进之后的效果到底怎么样吧。

                   

四、效果验证

        笔者使用和之前计时一样的套路对使用了Cython的list转成array步骤进行计时，在相近的数据量下得到耗时为：

        仅从这个对比来看，减少了大约25~26 ms时间，从百分比来看提升了91.15%，还是非常可观的提升，而且笔者这里仅是对list转array这一步进行了优化，后续还可尝试对上述步骤中耗时也很长的pcl_msg转list进行优化，那么这样一来对总体耗时来说是一个非常大的优化。