融合的pointpillars.detect代码注释

最新推荐文章于 2024-09-13 14:56:28 发布

努力把公司干倒闭

最新推荐文章于 2024-09-13 14:56:28 发布

阅读量142

点赞数

文章标签：聚类

本文链接：https://blog.csdn.net/m0_64346597/article/details/132197992

版权

#!/usr/bin/env python3 # 指定Python解释器的路径

# 导入必要的库和模块
import rospy                            # 导入ROS的Python库
from sensor_msgs.msg import PointCloud2  # 导入ROS的PointCloud2消息类型
import sensor_msgs.point_cloud2 as pc2   # 导入PointCloud2相关函数库
from std_msgs.msg import Header          # 导入ROS的Header消息类型
from pathlib import Path                 # 导入Path库，用于处理文件路径
from jsk_recognition_msgs.msg import BoundingBox, BoundingBoxArray # 导入BoundingBox消息类型

from time import time                    # 导入时间函数
import numpy as np                       # 导入NumPy库
from pyquaternion import Quaternion      # 导入四元数库
import scipy.linalg as linalg            # 导入SciPy的线性代数模块
import sys                               # 导入系统模块
import os                                # 导入操作系统模块

# 将自定义的路径加入系统路径
sys.path.append('/home/heven/pred_ws/src/pred_fusion/PointPillars')

# 获取当前文件的路径，并添加到系统路径中
FILE = Path(__file__).resolve()
ROOT = FILE.parents[0].parents[0].parents[0] / "PointPillars"
if str(ROOT) not in sys.path:
    sys.path.append(str(ROOT))          # 将根目录添加到PATH
ROOT = Path(os.path.relpath(ROOT, Path.cwd()))  # 获得相对路径

# 从自定义模块中导入模型
from model import PointPillars

# 再次将自定义的路径加入系统路径
sys.path.append('/home/heven/pred_ws/src/pred_fusion/OpenPCDet')

# 获取当前文件的路径，并添加到系统路径中
FILE = Path(__file__).resolve()
ROOT = FILE.parents[0].parents[0].parents[0] / "OpenPCDet"
if str(ROOT) not in sys.path:
    sys.path.append(str(ROOT))          # 将根目录添加到PATH
ROOT = Path(os.path.relpath(ROOT, Path.cwd()))  # 获得相对路径

# 从自定义模块中导入配置和工具
from pcdet.config import cfg, cfg_from_yaml_file
from pcdet.datasets import DatasetTemplate
from pcdet.utils import common_utils

# 定义一个用于筛选点云范围的函数
def point_range_filter(pts, point_range=[0, -39.68, -3, 69.12, 39.68, 1]):
    '''
    data_dict: dict(pts, gt_bboxes_3d, gt_labels, gt_names, difficulty)
    point_range: [x1, y1, z1, x2, y2, z2]
    '''
    # 各个方向上的条件筛选
    flag_x_low = pts[:, 0] > point_range[0]
    flag_y_low = pts[:, 1] > point_range[1]
    flag_z_low = pts[:, 2] > point_range[2]
    flag_x_high = pts[:, 0] < point_range[3]
    flag_y_high = pts[:, 1] < point_range[4]
    flag_z_high = pts[:, 2] < point_range[5]
    keep_mask = flag_x_low & flag_y_low & flag_z_low & flag_x_high & flag_y_high & flag_z_high
    pts = pts[keep_mask]
    return pts 

# 定义一个用于从四元数转换到欧拉角的函数
def quaternion2euluer(x, y, z, w):
    """
        Convert a quaternion into euler angles (roll, pitch, yaw)
        roll is rotation around x in radians (counterclockwise)
        pitch is rotation around y in radians (counterclockwise)
        yaw is rotation around z in radians (counterclockwise)
    """
    import math
    t0 = +2.0 * (w * x + y * z)
    t1 = +1.0 - 2.0 * (x * x + y * y)
    roll_x = math.atan2(t0, t1)
    t2 = +2.0 * (w * y - z * x)
    t2 = +1.0 if t2 > +1.0 else t2
    t2 = -1.0 if t2 < -1.0 else t2
    pitch_y = math.asin(t2)
    t3 = +2.0 * (w * z + x * y)
    t4 = +1.0 - 2.0 * (y * y + z * z)
    yaw_z = math.atan2(t3, t4)
    return math.degrees(yaw_z) # 返回以度为单位的偏航角

# 定义Demo数据集类，继承DatasetTemplate
class DemoDataset(DatasetTemplate):
    def __init__(self, dataset_cfg, class_names, training=True, root_path=None, logger=None, ext='.bin'):
        super().__init__(
            dataset_cfg=dataset_cfg, class_names=class_names, training=training, root_path=root_path, logger=logger
        )

# 定义Pointpillars_ROS类，封装了ROS与PointPillars模型的交互当然，以下是给出的代码的每一行的中文注释：

```python
#!/usr/bin/env python3 # 使用环境变量中的python3解释器执行此脚本

import rospy # 导入ROS的Python库
from sensor_msgs.msg import PointCloud2 # 从sensor_msgs包中导入PointCloud2消息类型
import sensor_msgs.point_cloud2 as pc2 # 导入sensor_msgs包中的point_cloud2模块
from std_msgs.msg import Header # 从std_msgs包中导入Header消息类型
from pathlib import Path # 导入Path库，用于处理文件和目录路径
from jsk_recognition_msgs.msg import BoundingBox, BoundingBoxArray # 从jsk_recognition_msgs包中导入BoundingBox和BoundingBoxArray消息类型

from time import time # 导入time模块的time函数，用于获取当前时间戳
import numpy as np # 导入NumPy库，并命名为np
from pyquaternion import Quaternion # 导入Quaternion库

import numpy as np # 再次导入NumPy库，这行代码是冗余的
import torch # 导入PyTorch库
import scipy.linalg as linalg # 导入SciPy库的线性代数模块，并命名为linalg
import sys # 导入系统库
import os # 导入操作系统库

sys.path.append('/home/heven/pred_ws/src/pred_fusion/PointPillars') # 将指定路径添加到系统路径中，使Python能够找到该目录下的模块

FILE = Path(__file__).resolve() # 获取当前文件的绝对路径
ROOT = FILE.parents[0].parents[0].parents[0] / "PointPillars" # 获取PointPillars的根路径
if str(ROOT) not in sys.path:
    sys.path.append(str(ROOT))  # 如果ROOT不在系统路径中，则将其添加
ROOT = Path(os.path.relpath(ROOT, Path.cwd()))  # 获取相对于当前工作目录的相对路径

from model import PointPillars # 从model模块中导入PointPillars类

sys.path.append('/home/heven/pred_ws/src/pred_fusion/OpenPCDet') # 将OpenPCDet的路径添加到系统路径中

FILE = Path(__file__).resolve() # 再次获取当前文件的绝对路径
ROOT = FILE.parents[0].parents[0].parents[0] / "OpenPCDet" # 获取OpenPCDet的根路径
if str(ROOT) not in sys.path:
    sys.path.append(str(ROOT))  # 如果ROOT不在系统路径中，则将其添加
ROOT = Path(os.path.relpath(ROOT, Path.cwd()))  # 获取相对于当前工作目录的相对路径

from pcdet.config import cfg, cfg_from_yaml_file # 从pcdet.config模块中导入cfg和cfg_from_yaml_file
from pcdet.datasets import DatasetTemplate # 从pcdet.datasets模块中导入DatasetTemplate类
from pcdet.utils import common_utils # 从pcdet.utils模块中导入common_utils

# 定义一个函数，用于过滤掉不在指定范围内的点
def point_range_filter(pts, point_range=[0, -39.68, -3, 69.12, 39.68, 1]):
    # ... 省略具体实现 ...

# 定义一个函数，用于将四元数转换为欧拉角
def quaternion2euluer(x, y, z, w):
    # ... 省略具体实现 ...

# 定义一个名为DemoDataset的类，继承自DatasetTemplate
class DemoDataset(DatasetTemplate):
    # ... 省略具体实现 ...

# 定义一个名为Pointpillars_ROS的类
class Pointpillars_ROS:
    # ... 省略具体实现 ...

# 如果此脚本作为主程序运行
if __name__ == '__main__':
    sec = Pointpillars_ROS() # 创建Pointpillars_ROS的实例
    
    try:
        rospy.spin() # 进入ROS的事件循环
    except KeyboardInterrupt: # 如果接收到键盘中断信号
        del sec # 删除sec实例
        print("Shutting down") # 打印“关闭”消息

轨迹预测

# 导入相关的库和依赖
import rospy
import torch
import sys
from pathlib import Path
import os

from time import time

# 导入消息类型
from jsk_recognition_msgs.msg import BoundingBoxArray
from visualization_msgs.msg import Marker, MarkerArray
from nav_msgs.msg import Odometry
from collections import deque

FILE = Path(__file__).resolve()  # 获取当前文件的绝对路径
ROOT = FILE.parents[0]
if str(ROOT) not in sys.path:
    sys.path.append(str(ROOT))  # 将ROOT添加到路径中
ROOT = Path(os.path.relpath(ROOT, Path.cwd()))  # 获取相对路径

import rviz_visualizer as visual  # 导入RViz可视化库

# 重新设置文件和根路径
FILE = Path(__file__).resolve()
ROOT = FILE.parents[0].parents[0].parents[0] / "crat-pred"
if str(ROOT) not in sys.path:
    sys.path.append(str(ROOT))  # 添加ROOT到路径
ROOT = Path(os.path.relpath(ROOT, Path.cwd()))  # 获取相对路径

from model.crat_pred import CratPred  # 从模型中导入CRAT-Pred

class Predictor():
    def __init__(self):

        # 初始化相关变量
        self.id_list = []
        self.global_trajectory = dict()
        self.global_trajectory[0] = []
        self.init_flag = 0
        self.ref_odom_x = 0
        self.centers_list = dict()
        self.past_trajectory = dict()

        # 初始化ROS节点
        rospy.init_node('prediction_node', anonymous=False)
        self.sensor_direction = rospy.get_param("~sensor_direction")
        self.crat_pred_path = rospy.get_param("~crat_pred_path")

        # 订阅传感器和轨迹数据
        rospy.Subscriber('fusion/tracks', BoundingBoxArray, self.tracking_callback)
        rospy.Subscriber('/Odometry', Odometry, self.odom_callback)
        self.traj_pub = rospy.Publisher('/trajectory', MarkerArray, queue_size=1)

    def odom_callback(self, msg):

        # 初始化起始位置，并保存当前位置
        if self.init_flag == 0:
            self.ref_odom_x = msg.pose.pose.position.x
            self.init_flag = 1

        self.odom_x = msg.pose.pose.position.x - self.ref_odom_x
        self.odom_y = msg.pose.pose.position.y

        self.cur_time = msg.header.stamp.secs + msg.header.stamp.nsecs * 1e-9

    def tracking_callback(self, data):

        # 存储物体边界框并预测
        self.object_boxes = data.boxes
        self.prediction()

    def prediction(self):

        # 根据时间间隔进行预测
        if (self.cur_time % 0.1) > 0.09 or (self.cur_time % 0.1) < 0.01:

            # 这一段代码处理车辆和物体的位置和方向，并将其转换为相对位移
            # 进一步处理将用于多代理轨迹预测的数据结构

            # 利用队列进行轨迹预测，并使用CRAT-Pred模型进行推理
            # 将预测结果转换为局部坐标，并通过RViz进行可视化

    def position_2_displ(self, global_traj):

        # 将全局坐标转换为位移
        displ = []
        for index in range(len(global_traj)-1):
            displ.append([global_traj[index+1][0] - global_traj[index][0], global_traj[index+1][1] - global_traj[index][1], 1.0000])

        return displ

if __name__ == "__main__":

    # 如果ROS未关闭，则启动预测器并开始监听
    if not rospy.is_shutdown():
        Predictor()
        rospy.spin()

launch

<launch>
    <!-- YOLO Detection configuration -->  <!-- YOLO 检测配置 -->
    <arg name="weights" default="/home/heven/pred_ws/src/pred_fusion/fusion_prediction/yolo.pt"/> <!-- 权重文件路径 -->
    <arg name="data" default="/home/heven/pred_ws/src/pred_fusion/fusion_prediction/src/yolov5/data/coco128.yaml"/> <!-- 数据配置文件路径 -->
    <arg name="confidence_threshold" default="0.6"/> <!-- 置信度阈值 -->
    <arg name="iou_threshold" default="0.45"/> <!-- 交集联合比阈值 -->
    <arg name="maximum_detections" default="1000"/> <!-- 最大检测数量 -->
    <arg name="device" default="0"/> <!-- 设备编号 -->
    <arg name="agnostic_nms" default="true"/> <!-- 是否进行agnostic非极大值抑制 -->
    <arg name="line_thickness" default="3"/> <!-- 线条粗细 -->
    <arg name="dnn" default="true"/> <!-- 是否使用深度神经网络 -->
    <arg name="half" default="false"/> <!-- 是否进行半精度计算 -->
    <arg name="inference_size_h" default="640"/> <!-- 推理图像高度 -->
    <arg name="inference_size_w" default="640"/> <!-- 推理图像宽度 -->
    <arg name="view_image" default="true"/> <!-- 是否显示图像 -->
    <arg name="input_image_topic" default="/carmaker_vds_client_node/image_raw/compressed"/> <!-- 输入图像的主题 -->
    <arg name="output_topic" default="/yolov5/detections"/> <!-- 输出主题 -->
    <arg name="publish_image" default="true"/> <!-- 是否发布图像 -->
    <arg name="output_image_topic" default="/yolov5/image_out"/> <!-- 输出图像的主题 -->

    <!-- PointPillars configurations --> <!-- PointPillars 配置 -->
    <arg name="config_path" default="/home/heven/pred_ws/src/pred_fusion/OpenPCDet/tools/cfgs/kitti_models/pointpillar.yaml"/> <!-- 配置文件路径 -->
    <arg name="ckpt_path" default="/home/heven/pred_ws/src/pred_fusion/fusion_prediction/pillars.pth"/> <!-- 检查点文件路径 -->
    <arg name="input_lidar_topic" default="/pointcloud/os1_pc2"/> <!-- 输入雷达主题 -->
    <arg name="score_threshold" default="0.6"/> <!-- 分数阈值 -->

    <!-- Fusion configurations --> <!-- 融合配置 -->
    <arg name="fusion_iou_threshold" default="0.2"/> <!-- 融合的交集联合比阈值 -->

    <!-- Tracker configurations --> <!-- 追踪器配置 -->
    <arg name="tracker_iou_threshold" default="0.1"/> <!-- 追踪器的交集联合比阈值 -->

    <!-- Predictor configurations --> <!-- 预测器配置 -->
    <arg name="crat_pred_path" default="/home/heven/pred_ws/src/pred_fusion/fusion_prediction/crat.ckpt"/> <!-- 预测器文件路径 -->
    <arg name="sensor_direction" default="rear"/> <!-- 传感器方向 -->

    <node pkg="fusion_prediction" name="detector" type="detector.py" output="screen"> <!-- 检测器节点配置 -->
        <!-- 各种参数设置，同上 -->
    </node>

    <node pkg="fusion_prediction" name="pillar_node" type="pillars_detect.py" output="screen"> <!-- Pillar检测节点配置 -->
        <!-- 各种参数设置，同上 -->
    </node>

    <node pkg="fusion_prediction" name="fusion_node" type="fusion.py" output="screen"> <!-- 融合节点配置 -->
        <!-- 参数设置，同上 -->
    </node>
    
    <node pkg="rviz" name="rviz" type="rviz" args="-d $(find fusion_prediction)/launch/fusion_display.rviz" /> <!-- rviz显示节点配置 -->
    
    <node pkg="fusion_prediction" name="tracker_node" type="lidar_tracker.py" output="screen"> <!-- 雷达追踪节点配置 -->
        <!-- 参数设置，同上 -->
    </node>

    <node pkg="fusion_prediction" name="prediction_node" type="predict.py" output="screen"> <!-- 预测节点配置 -->
        <!-- 参数设置，同上 -->
    </node>

</launch>