Autoware感知瞎学笔记（一）lidar_kf_contour_track

最新推荐文章于 2024-04-18 17:01:37 发布

AMG-C63er

最新推荐文章于 2024-04-18 17:01:37 发布

阅读量4.5k

点赞数 7

CC 4.0 BY-SA版权

分类专栏： Autoware 文章标签： c++

本文链接：https://blog.youkuaiyun.com/YP233/article/details/116708489

Autoware感知瞎学笔记（一）lidar_kf_contour_track

目录
总结

三维激光雷达数据从传感器出来后，经过传统的聚类+高精地图object filter，或者learning-based的方法之后，得到一个目标列表，此目标列表含有二维位置信息和角度信息，没有速度信息；同时基于每一帧的检测方法，可能会出现“目标消失”的问题，因此需要滤波器来track住这些目标，下面看Autoware是怎么处理这个问题的。
代码位置：autoware/core_perception/lidar_kf_contour_track

1. lidar_kf_contour_track.cpp

拿到ros的package，直接打开CMakeList文件
在这里插入图片描述
可以看到构成lidar_kf_contour_track这个节点的文件，有四个。我们直奔主题打开每个cpp，可以看到lidar_kf_contour_track.cpp是这个节点的入口，在main函数内实例化在ContourTrackerNS命名空间内的一个类ContourTracker，调用MainLoop() 方法。因此打开定义ContourTracker类的头文件：lidar_kf_contour_tracker_core.h
在这里插入图片描述

2. lidar_kf_contour_tracker_core.h

我们在这里首先分析这个入口类ContourTracker的头文件，并且从类内变量逐行分析
在这里插入图片描述 1. std::vector< PlannerHNS::DetectedObject > m_OriginalClusters;
打开DetectedObject这一类型定义：该文件存放在common/op_planner/RoadNetwork.h中

里面存放这描述检测目标的一些基本属性，标签，类别，尺寸，速度，角度，等等，，。同时在构造函数的时候，初始化这些变量

2.autoware_msgs::DetectedObjectArray m_OutPutResults;
记录发布数据的ros消息，消息格式如下：

std_msgs/Header header
DetectedObject[] objects

DetectedObject消息格式：
可以看到是将3D BoundingBox和目标在2D图像中的大小都汇总在此消息数据里，便于目标消息的复用。

std_msgs/Header                 header

uint32                          id
string                          label
float32                         score   #Score as defined by the detection, Optional
std_msgs/ColorRGBA              color   # Define this object specific color
bool                            valid   # Defines if this object is valid, or invalid as defined by the filtering

################ 3D BB
string                          space_frame #3D Space coordinate frame of the object, required if pose and dimensions are defines
geometry_msgs/Pose              pose
geometry_msgs/Vector3           dimensions
geometry_msgs/Vector3           variance
geometry_msgs/Twist             velocity
geometry_msgs/Twist             acceleration

sensor_msgs/PointCloud2         pointcloud

geometry_msgs/PolygonStamped    convex_hull
autoware_msgs/LaneArray         candidate_trajectories

bool                            pose_reliable
bool                            velocity_reliable
bool                            acceleration_reliable

############### 2D Rect
string                          image_frame # Image coordinate Frame,        Required if x,y,w,h defined
int32                           x           # X coord in image space(pixel) of the initial point of the Rect
int32                           y           # Y coord in image space(pixel) of the initial point of the Rect
int32                           width       # Width of the Rect in pixels
int32                           height      # Height of the Rect in pixels
float32                         angle       # Angle [0 to 2*PI), allow rotated rects

sensor_msgs/Image               roi_image

############### Indicator information
uint8                          indicator_state # INDICATOR_LEFT = 0, INDICATOR_RIGHT = 1, INDICATOR_BOTH = 2, INDICATOR_NONE = 3

############### Behavior State of the Detected Object
uint8                           behavior_state # FORWARD_STATE = 0, STOPPING_STATE = 1, BRANCH_LEFT_STATE = 2, BRANCH_RIGHT_STATE = 3, YIELDING_STATE = 4, ACCELERATING_STATE = 5, SLOWDOWN_STATE = 6

#
string[]                        user_defined_info

3.bool bNewClusters;
bool类型变量, 表达是否是新的cluster

4.PlannerHNS::WayPoint m_CurrentPos; 使用WayPoint类型来表示当前的位姿
其中waypoint记录了当前所属的lane ID，id，Left ID， Right ID，前面的和后面的waypoints。这里因为作者还没有阅读整个autoware的代码，所以在这大胆猜测一下，这里记录的是当前lane内，在此waypoint的前面和后面所有的waypoints，和carla里面导航的方式是一样的。同时记录了关于occupancy grid map 的参数。关于其他的参数含义，我们在后续的文章中再去解读。
在这里插入图片描述 5.bool bNewCurrentPos;
这里表示的应该是是否收到了新的位姿信息，表示位置更新

6.PerceptionParams m_Params;
看到变量的名字感觉是感知相关的参数，但是看内容其实这里的参数感觉是滤波相关的参数，车辆的size，检测半径，最大的objectsize，最小的objectsize，目标boundingbox相关参数，其他的应该是滤波器的相关设置
在这里插入图片描述 7.SimpleTracker m_ObstacleTracking;
这里便是SimpleTracker实际的调用了，这个类其实是将kF又封装了一层，用于管理检测到的对象。关于SimpleTracker我们在第二节分析。
8.这部分的成员变量是用来表示可视化的

9. std::vector < std::string > m_LogData; 记录log数据
10.PlannerHNS::MAP_SOURCE_TYPE m_MapType; 地图的类型
11.std::string m_MapPath;存放地图的路径
12.PlannerHNS::RoadNetwork m_Map;
这里存放的是便是autoware的高精地图，vector_map，我们打开RoadNetWork的类定义看下：
在这里插入图片描述
可以看到autoware使用vector来存放同一类型的道路元素，关于底层基类比如RoadSegament，TrafficLight，StopLine的分析，会在后续的文章中讲解。同时我们也会去学习autoware中的高精地图是怎样做的，它是怎么提供我们所需要的信息的。
13.bool bMap;
14.double m_MapFilterDistance;
15.std::vector < PlannerHNS::Lane*> m_ClosestLanesList;
在这里我们打开PlannerHNS::Lane这个数据类型：
在这里插入图片描述
这里分别有如下属性：
id，roadId当前lane所在road的id，areaId，fromAreaId，toAreaId，
fromIds表示当前lane的父lane Id，
toIds表示当前lane的子lane Id，
num表示一个road内有多少条lane
speed，dir，type，
points这个车道内所有的路点，trafficlights 交通灯，stoplines停止线，
fromLanes， toLanes 则是刚刚ID对应的道路集合
pLeftLane, pRightLane 这是左右两边的道路
pRoad则是当前lane所在的road

16.这里是类内关于kf滤波器的一些参数了
在这里插入图片描述
17. ros层变量以及回调函数声明
18.最后公有变量里面都是关于map的，通过ros topic的方式接收地图各种元素消息。

3. lidar_kf_contour_tracker_core.cpp

3.1 构造函数ContourTracker()

在这里插入图片描述

第一部分初始化类内成员变量
ReadNodeParams():通过ros参数服务器拿到参数
ReadCommonParams()：这里bEnableLaneChange参数需要记住，还有上面的m_MapFilterDistance 参数，下面要考。
设置并且初始化kf滤波器

最后的初始化部分，会在第二节SimpleTracker部分分析，根据函数名字可以猜到，这是初始化滤波器。
订阅和发布topic
这里让人注意的应该是发布ttc_direct_path这个topic，单学习过感知但是没有无人驾驶知识的，应该不知道ttc这个知识点，ttc的全称是time to crash，就是剩余到达要碰撞的时间。因此读到这里，感觉下面是要计算出一个到达前面目标的路径，然后可视化出来。
下面便是关于可视化的一些操作
打开InitMarkers()函数:
其实这里就是通过调用CreateGenMarker的方法生成五种类型的显示Marker，分别用来显示目标的中心，方向，文本标记，外围的polygon，还有track出来的轨迹。打开CreateGenMarker方法：
这里传入相应marker的属性标签，返回对应的marker。
因此我们可以得知，类内全局变量m_DetectedPolygonsDummy 和 m_DetectedPolygonsActual 存放的是五种关于目标属性的Marker。
最后调用InitMatchingMarkers() 方法：
其实这里也是换汤不换药，调用CreateMarker方法生成用于显示匹配关系的Marker，然后类型是LINE_STRIP

3.2 析构函数 ~ContourTracker()

在这里插入图片描述在析构的时候，我们需要在某个位置写入log数据，记录跟踪对象的详细数据

3.3 点云聚类回调函数

在这里插入图片描述

首先看下消息格式类型：autoware_msgs::CloudClusterArrayConstPtr，里面存放的是标准的消息头，还有CloudCluster类型的数组

打开CloudCluster消息类型，查看里面的格式：

分别是标准消息头，id，标签，得分，点云消息，估计角度，点云的最小值，最大值，平均值，中心值，三个尺度量（这里暂时不清楚存放的是什么变量），点云fpfh描述子，boundingbox，描述凸边界的convex_hull，目标转向信息。

3.3.1 ImportCloudClusters函数

如果有定位数据 || 使用仿真的方式
调用ImportCloudClusters(msg, m_OtiginalClusters) 方法, 将聚类的数据转到类内全局变量m_OriginalClusters中

先是初始化一些变量，然后是实例化一个PolygonGenrator类，打开PolygonGenrator类定义：

class PolygonGenerator
{
   
   
public:

	PlannerHNS::GPSPoint m_Centroid;
	std::vector<QuarterView> m_Quarters;
	std::vector<PlannerHNS::GPSPoint> m_Polygon;

	PolygonGenerator(int nQuarters);
	virtual ~PolygonGenerator();
	std::vector<QuarterView> CreateQuarterViews(const int& nResolution);
	std::vector<PlannerHNS::GPSPoint> EstimateClusterPolygon(const pcl::PointCloud<pcl::PointXYZ>& cluster, 
		const PlannerHNS::GPSPoint& original_centroid, PlannerHNS::GPSPoint& new_centroid, const double& polygon_resolution = 1.0);
};

打开类定义CPP文件PolygonGenerator.cpp,首先看到构造函数部分：

std::vector<QuarterView> PolygonGenerator::CreateQuarterViews(const int& nResolution)
{
   
   
  std::vector<QuarterView> quarters;
  if(nResolution <= 0)
    return quarters;

  double range = 360.0 / nResolution;
  double angle = 0;
  for(int i = 0; i < nResolution; i++)
  {
   
   
    QuarterView q(angle, angle+range, i);
    quarters.push_back(q);
    angle+=range;
  }

  return quarters;
}

这里面构造函数返回值是：std::vector< QuaterView >，我们就先看下QuarterView类定义：
在这里插入图片描述

构造函数和Init方法都可以传入参数
Reset方法重置标志位
UpdateQuarterView（）方法，如果传入的路点的角度超出角度范围限制，则不更新。如果bFirst为真，即第一次更新数据，则直接将点赋值给类内全局变量max_from_center，如果更新过了那么只有传入点的cost大于类内max_from_center的cost，才覆盖max_from_center。
GetMaxPoint() 方法，如果没有更新过UpdateQuarterView方法，则返回false，即拿不到最大点。否则取得类内全局变量max_from_center。

这里再回到CreateQuarterViews方法，先定义一个QuarterView类型的vector，如果传入nResolution小于0，则直接返回。将360度分成nResolution等份，实例化nResolution个QuarterView，每个QuarterView平分360度，并存入到quarters，返回。

std::vector<QuarterView> PolygonGenerator::CreateQuarterViews(const int& nResolution)
{
   
   
  std::vector<QuarterView> quarters;
  if(nResolution <= 0)
    return quarters;

  double range = 360.0 / nResolution;
  double angle = 0;
  for(int i = 0; i < nResolution; i++)
  {
   
   
    QuarterView q(angle, angle+range, i);
    quarters.push_back(q);
    angle+=range;
  }

  return quarters;
}

继续打开PolygonGenerator.cpp分析，下面是主要的一个函数EstimateClusterPolygon：

std::vector<PlannerHNS::GPSPoint> PolygonGenerator::EstimateClusterPolygon(const pcl::PointCloud<pcl::PointXYZ>& cluster, const PlannerHNS::GPSPoint& original_centroid, PlannerHNS::GPSPoint& new_centroid, const double& polygon_resolution)
{
   
   
  for(unsigned int i=0; i < m_Quarters.size(); i++)
      m_Quarters.at(i).ResetQuarterView();

  PlannerHNS::WayPoint p;
  for(unsigned int i=0; i< cluster.points.size(); i++)
  {
   
   
    p.pos.x = cluster.points.at(i).x;
    p.pos.y = cluster.points.at(i).y;
    p.pos.z = original_centroid.z;

    PlannerHNS::GPSPoint v(p.pos.x - original_centroid.x , p.pos.y - original_centroid.y, 0, 0);
    p.cost = pointNorm(v);
    p.pos.a = UtilityHNS::UtilityH::FixNegativeAngle(atan2(v.y, v.x))*(180. / M_PI);

    for(unsigned int j = 0 ; j < m_Quarters.size(); j++)
    {
   
   
      if(m_Quarters.at(j