autoware.universe源码略读(3.10)--perception:lidar_apollo_instance_segmentation
autoware.universe源码略读3.10--perception:lidar_apollo_instance_segmentation
Overview
这里是对激光雷达点云数据进行神经网络处理的模块，具体说是根据基于 CNN 的模型和障碍物聚类方法，将激光雷达传感器的三维点云数据分割为障碍物，如汽车、卡车、自行车和行人。 这里用到的是Apollo的软件包，所以算法的思路和细节要参考Apollo文档。在文件中，放置了三个模型，VLP-16，HDL-64，VLS-128，根据描述是Supported lidars are velodyne 16, 64 and 128, but you can also use velodyne 32 and other lidars with good accuracy. 所以如果没有特殊需求的话应该就不需要再训练新的模型出来吧。对于更多的细节，则要参考以下三个链接了：
1. 3D 障碍物感知
2. TRTWrapper
3. CNN LiDAR Baidu Object Segmenter
这里来稍微仔细地看下这个模块吧
lib
这个文件夹应该和之前提到过的YOLO中的lib文件夹用处差不多，应该也是关于TensorRT部署的东西，在Utils.hpp里定义了一些辅助的类和函数
| 类/函数名 | 作用 | 父类 |
|---|---|---|
Profiler | 看起来是对对每层执行时间的记录 | nvinfer1::IProfiler |
Logger | 输出调试信息等级的控制 | nvinfer1::ILogger |
CUDA_CHECK | 抛出CUDA错误的 | |
write | 写字节进buffer | |
read | 读取buffer中的字节 |
然后在TrtNet.hpp中，定义了神经网络相关的类trtNet，需要输入的外部参数就是引擎文件const std::string & engineFile，构造函数里对文件的读取是先移动到文件尾得到一个总长度，再移回文件头读取对应长度的字节
file.seekg(0, ios::end); // go to the end
int length = file.tellg(); // report location (this is the length)
file.seekg(0, ios::beg); // go back to the beginning
std::unique_ptr<char[]> data(new char[length]);
file.read(data.get(), length);
file.close();
接下来就是创建一个推理引擎了
mTrtRunTime = createInferRuntime(gLogger);
assert(mTrtRunTime != nullptr);
mTrtEngine = mTrtRunTime->deserializeCudaEngine(data.get(), length);
assert(mTrtEngine != nullptr);
最后是调用InitEngine函数来对引擎初始化，在函数中先初始化了执行上下文的变量
const int maxBatchSize = 1;
/// @var mTrtContext 执行上下文
mTrtContext = mTrtEngine->createExecutionContext();
assert(mTrtContext != nullptr);
/// @var mTrtProfiler 执行上下文的分析器
mTrtContext->setProfiler(&mTrtProfiler);
然后获取得到绑定数量，关于绑定数量其实自己一直不太理解，这里又去学习了一下，指的应该是模型中所有输入和输出的数量，如果一个模型有一个输入量和一个输出量，那这个绑定数量应该就是2。
int nbBindings = mTrtEngine->getNbBindings();
之后对绑定的遍历，又设置了一些成员变量
mTrtCudaBuffer.resize(nbBindings); // 存储每个绑定的 CUDA 缓冲区指针
mTrtBindBufferSize.resize(nbBindings); // 存储每个绑定的缓冲区大小
for (int i = 0; i < nbBindings; ++i) {
Dims dims = mTrtEngine->getBindingDimensions(i);
DataType dtype = mTrtEngine->getBindingDataType(i);
int64_t totalSize = volume(dims) * maxBatchSize * getElementSize(dtype);
mTrtBindBufferSize[i] = totalSize;
mTrtCudaBuffer[i] = safeCudaMalloc(totalSize);
if (mTrtEngine->bindingIsInput(i)) {
mTrtInputCount++; // 输入绑定的数量
}
}
最后一个函数就是执行推理的函数doInference了，到这里反而代码的逻辑反而比较简单了，因为把相关的变量和参数都设置好了，这里直接调用TensorRT库的函数就可以了，核心是执行上下文的execute函数，还有一个cudaMemcpyAsync函数，这个函数是把数据从主机内存复制到了设备内存（所以就是复制到显卡了吧），最后把输出的输出就是output
mTrtContext->execute(batchSize, &mTrtCudaBuffer[inputIndex]);
总的看下来这里只是为神经网络部署做了一些准备工作以及函数接口，具体的网络部署还是要看调用的时候。接下来看一下lidar_apollo_instance_segmentationl里定义的一些其他文件和函数
util
util.hpp里定义了三个函数，其中F2I是把浮点型的数据根据原点ori和尺度scale转换为了整型，然后Pc2Pixel就是把世界坐标系尺度下的点云数据转换到了图像尺度下，而Pixel2Pc则是反过来，把图像尺度下的点云数据再转到世界坐标系尺度下。
log_table
这个看起来是存放了一个自然对数数值的结果的表，在构造函数里是直接调用std::log1p来计算了自然对数
LogTable()
{
data.resize(256 * 10);
for (size_t i = 0; i < data.size(); ++i) {
data[i] = std::log1p(static_cast<float>(i / 10.0));
}
然后calcApproximateLog就是计算自然对数的，里面就是判断在不在已经生成的表里，如果在的话直接取表对应的索引就行，不再的话再调用std::log(static_cast<float>(1.0 + num))来计算
debuggger
其实这个没什么好说的，发布的是调试信息，不过这里发布的是点云的距离信息
instance_pointcloud_pub_ =
node->create_publisher<sensor_msgs::msg::PointCloud2>("debug/instance_pointcloud", 1);
这里主要是对最后检测到的结果，根据不同的类别赋值了不同的颜色，最后发布的点就包含了xyz坐标和rgb颜色信息
pcl::PointXYZRGB colored_point;
colored_point.x = object_pointcloud[i].x;
colored_point.y = object_pointcloud[i].y;
colored_point.z = object_pointcloud[i].z;
colored_point.r = red;
colored_point.g = green;
colored_point.b = blue;
colored_pointcloud.push_back(colored_point);
disjoint_set
这里实现的是不相交集合（disjoint-set datastructure）数据结构又被称为并查集（union-find data structure）数据结构。主要用于处理一些不相交的动态集合，并支持两种主要操作：查找（Find）和合并（Union）。关于并查集的具体概念，可以参考【算法与数据结构】—— 并查集，这里面有很多函数，依次来分析一下
DisjointSetMakeSet：看起来就是把自己变成了一个集合，这是父节点也是自己，节点数量还是0DisjointSetFindRecursive： 通过递归的形式，找到了根节点DisjointSetFind： 好像和上边的作用是一样的。。感觉也是找到了根节点DisjointSetUnion： 合并两个集合，判断谁的根节点的node_rank更高，选择更高的一个作为父节点，相同的话就默认x为父节点
feature_map
这里定义的是特征图结构，但是分成了几种类型，分别是最基本的FeatureMap，带有点云强度信息的FeatureMapWithIntensity，然后是连续特征图（应该是这个意思吧）FeatureMapWithConstant以及包含了这两种特性的FeatureMapWithConstantAndIntensity，不同的类型就是里面包含的数据不一样，其中在FeatureMapWithConstant初始化特征图的时候，直接计算了每个索引到网格中心的距离和方向(也不知道这样理解对不对)，这里的60不知道是不是经验值
void FeatureMapWithConstant::initializeMap([[maybe_unused]] std::vector<float> & map)
{
for (int row = 0; row < height; ++row) {
for (int col = 0; col < width; ++col) {
int idx = row * width + col;
// * row <-> x, column <-> y
// return the distance from my car to center of the grid.
// Pc means point cloud = real world scale. so transform pixel scale to
// real world scale
float center_x = Pixel2Pc(row, height, range);
float center_y = Pixel2Pc(col, width, range);
// normalization. -0.5~0.5
direction_data[idx] = static_cast<float>(std::atan2(center_y, center_x) / (2.0 * M_PI));
distance_data[idx] = static_cast<float>(std::hypot(center_x, center_y) / 60.0 - 0.5);
}
}
}
FeatureGenerator
上一个是对特征图的定义，那这里就是特征的生成了，根据参数的设置，在构造函数实例化不同类型的特征图并完成初始化。
而具体的生成的函数就是generate了，这个函数很简单，就是遍历输入的点云数据，然后把点云数据转换到特征图网格上，然后更新对应位置的特征图的值，没有什么需要特别看的东西
Cluster2D
这个应该就是点云聚类的东西了，首先里面将障碍物再打包成了一个结构体Obstacle
MetaType meta_type; // 障碍物类型的信息
std::vector<float> meta_type_probs; // 障碍物类型的概率
这里面首先有个函数是traverse，这个函数的作用是遍历一个节点链，将每一个节点的traversed成员变量设置为1，将每一个节点的parent成员变量设置为链的中心节点，以及将链的中心节点和链中的某些节点的is_center成员变量设置为true。后边很多地方都用到了这个节点链遍历的函数
然后在主要的cluster函数中，一上来是对网格的初始化，初始化之后遍历有效的点云数据，检查点对应的网格位置是否有效，如果有效，将点云索引映射到网格索引，并增加对应网格的点数。
for (size_t i = 0; i < valid_indices_in_pc_->size(); ++i) {
int point_id = valid_indices_in_pc_->at(i);
const auto & point = pc_ptr_->points[point_id];
// * the coordinates of x and y have been exchanged in feature generation
// step,
// so we swap them back here.
int pos_x = F2I(point.y, range_, inv_res_x_); // col
int pos_y = F2I(point.x, range_, inv_res_y_); // row
if (IsValidRowCol(pos_y, pos_x)) {
point2grid_[i] = RowCol2Grid(pos_y, pos_x);
nodes[pos_y][pos_x].point_num++;
}
}
接下来遍历网格，初始化每个节点（Node），并且使用DisjointSetMakeSet初始化每个节点，再根据阈值判断节点是否为目标物，并且计算每个节点的中心节点（根据实例预测数据进行偏移）。
for (int row = 0; row < rows_; ++row) {
for (int col = 0; col < cols_; ++col) {
int grid = RowCol2Grid(row, col);
Node * node = &nodes[row][col];
DisjointSetMakeSet(node);
node->is_object = (use_all_grids_for_clustering || nodes[row][col].point_num > 0) &&
(*(category_pt_data + grid) >= objectness_thresh);
int center_row = std::round(row + instance_pt_x_data[grid] * scale_);
int center_col = std::round(col + instance_pt_y_data[grid] * scale_);
center_row = std::min(std::max(center_row, 0), rows_ - 1);
center_col = std::min(std::max(center_col, 0), cols_ - 1);
node->center_node = &nodes[center_row][center_col];
}
}
然后遍历网格，利用traverse进行标记；再遍历网格，对每个中心节点的相邻中心节点进行合并。所以这里其实可以理解为是聚类的操作吧？就是把一些相邻的中心节点并且进行了合并，这里的代码括号太多了，就不贴了
最后就是遍历网格，生成障碍物并对每个网格进行标记。然后再调用两个函数分别进行过滤和分类
for (int row = 0; row < rows_; ++row) {
for (int col = 0; col < cols_; ++col) {
Node * node = &nodes[row][col];
if (!node->is_object) {
continue;
}
Node * root = DisjointSetFind(node);
if (root->obstacle_id < 0) {
root->obstacle_id = count_obstacles++;
obstacles_.push_back(Obstacle());
}
int grid = RowCol2Grid(row, col);
id_img_[grid] = root->obstacle_id;
obstacles_[root->obstacle_id].grids.push_back(grid);
}
}
filter(inferred_data);
classify(inferred_data);
这里的filter函数其实就是根据推理得到的结果inferred_data以及刚刚聚类得到的障碍物结果obstacles_，设置分数、高度、朝向这些信息
for (size_t obstacle_id = 0; obstacle_id < obstacles_.size(); obstacle_id++) {
Obstacle * obs = &obstacles_[obstacle_id];
double score = 0.0;
double height = 0.0;
double vec_x = 0.0;
double vec_y = 0.0;
for (int grid : obs->grids) {
score += static_cast<double>(confidence_pt_data[grid]);
height += static_cast<double>(height_pt_data[grid]);
vec_x += heading_pt_x_data[grid];
vec_y += heading_pt_y_data[grid];
}
obs->score = score / static_cast<double>(obs->grids.size());
obs->height = height / static_cast<double>(obs->grids.size());
obs->heading = std::atan2(vec_y, vec_x) * 0.5;
obs->cloud_ptr.reset(new pcl::PointCloud<pcl::PointXYZI>);
}
然后classify就是进行分类的，推理的时候应该是有得到对应的概率的
const float * classify_pt_data = inferred_data.get() + siz_ * 4;
最后就是把概率最高的一个设为当前的meta_type了
for (int k = 0; k < num_classes; k++) {
obs->meta_type_probs[k] /= obs->grids.size();
if (obs->meta_type_probs[k] > obs->meta_type_probs[meta_type_id]) {
meta_type_id = k;
}
}
obs->meta_type = static_cast<MetaType>(meta_type_id);
detector
这里定义的类是LidarApolloInstanceSegmentation，这个类是继承自最后的节点类的，所以本质上还是一个节点类，构造函数还是加载了很多的参数：
| Name | Type | Default Value | Description |
|---|---|---|---|
score_threshold | double | 0.8 | If the score of a detected object is lower than this value, the object is ignored. |
range | int | 60 | Half of the length of feature map sides. [m] |
width | int | 640 | The grid width of feature map. |
height | int | 640 | The grid height of feature map. |
engine_file | string | “vls-128.engine” | The name of TensorRT engine file for CNN model. |
prototxt_file | string | “vls-128.prototxt” | The name of prototxt file for CNN model. |
caffemodel_file | string | “vls-128.caffemodel” | The name of caffemodel file for CNN model. |
use_intensity_feature | bool | true | The flag to use intensity feature of pointcloud. |
use_constant_feature | bool | false | The flag to use direction and distance feature of pointcloud. |
target_frame | string | “base_link” | Pointcloud data is transformed into this frame. |
z_offset | int | 2 | z offset from target frame. [m] |
接下来加载引擎文件，如果引擎文件加载成功的话，我们只需要实例化在lib中定义过的trtNet类就可以了
net_ptr_.reset(new Tn::trtNet(engine_file));
加载失败的话还挺复杂的，看起来是根据prototxt_file和caffemodel_file两个文件，直接搞出来了一个引擎文件，相当于重新生成一个engine_file，这里搜索了一下应该是涉及到了Caffe架构，这个自己就不太了解了，先mark两篇文章看看
所以这里相当于是调用TensorRT的库，直接根据两个文件生成了一个网络结构
const nvcaffeparser1::IBlobNameToTensor * blob_name2tensor = parser->parse(
prototxt_file.c_str(), caffemodel_file.c_str(), *network, nvinfer1::DataType::kFLOAT);
std::string output_node = "deconv0";
auto output = blob_name2tensor->find(output_node.c_str());
if (output == nullptr) {
RCLCPP_ERROR(node_->get_logger(), "can not find output named %s", output_node.c_str());
}
network->markOutput(*output);
#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8400
config->setMemoryPoolLimit(nvinfer1::MemoryPoolType::kWORKSPACE, 1 << 30);
#else
config->setMaxWorkspaceSize(1 << 30);
#endif
nvinfer1::IHostMemory * plan = builder->buildSerializedNetwork(*network, *config);
assert(plan != nullptr);
std::ofstream outfile(engine_file, std::ofstream::binary);
assert(!outfile.fail());
outfile.write(reinterpret_cast<char *>(plan->data()), plan->size());
outfile.close();
最后分别又实例化了两个类，分别是FeatureGenerator进行特征生成的，Cluster2D则应该是2D点云聚类的
transformCloud是对输入的点云进行了坐标转换，坐标转换的部分其实没什么好说的，也是找到两个坐标之间的转换关系，然后直接转换就好了，不过这里后边有一步时考虑Z轴偏移，这里的Z轴指的是目标框架中Z轴的偏移，没有完全get到这个概念，我的想法是莫非指的是雷达的安装高度？这样似乎算是在Z轴的一个偏移
pcl::PointCloud<pcl::PointXYZI> pointcloud_with_z_offset;
Eigen::Affine3f z_up_translation(Eigen::Translation3f(0, 0, z_offset));
Eigen::Matrix4f z_up_transform = z_up_translation.matrix();
pcl::transformPointCloud(pcl_transformed_cloud, pcl_transformed_cloud, z_up_transform);
这里最后的一个函数就是detectDynamicObjects了，先生成特征图
// generate feature map
std::shared_ptr<FeatureMapInterface> feature_map_ptr =
feature_generator_->generate(pcl_pointcloud_raw_ptr);
然后执行推理的过程
// inference
std::shared_ptr<float> inferred_data(new float[net_ptr_->getOutputSize() / sizeof(float)]);
net_ptr_->doInference(feature_map_ptr->map_data.data(), inferred_data.get());
然后进行后处理，根据推理检测的结果进行聚类
// post process
const float objectness_thresh = 0.5;
pcl::PointIndices valid_idx;
valid_idx.indices.resize(pcl_pointcloud_raw_ptr->size());
std::iota(valid_idx.indices.begin(), valid_idx.indices.end(), 0);
cluster2d_->cluster(
inferred_data, pcl_pointcloud_raw_ptr, valid_idx, objectness_thresh,
true /*use all grids for clustering*/);
const float height_thresh = 0.5;
const int min_pts_num = 3;
cluster2d_->getObjects(
score_threshold_, height_thresh, min_pts_num, output, transformed_cloud.header);
最后就是把类型再转换会去，然后给输出output赋值了
node
最后来看这个节点类，之前也有提到，detector中的类其实是继承自最后的节点类的，而这个节点类就是很标准的那种了，是最后会被调用的那种接口。可以在构造函数中看到，对前面提到的LidarApolloInstanceSegmentation类都具体的实例化的步骤，剩下的也就是话题的订阅和发布，这里定于的话题就是点云数据，而发布的则是最后的动态障碍物的检测结果了。
至于这里的点云回调函数其实也非常简单，因为之前LidarApolloInstanceSegmentation类已经被实例化了，所以这里只需要调用对应的接口函数就好了
tier4_perception_msgs::msg::DetectedObjectsWithFeature output_msg;
detector_ptr_->detectDynamicObjects(*msg, output_msg);
dynamic_objects_pub_->publish(output_msg);
总结
这里是对点云数据进行障碍物检测的软件包，里面也是用到了神经网络的东西，不过我的理解还是把点云数据线转换成特征图然后进行网络推理的，关于用到的网络具体是怎么做的可能还不是很清晰，不过大体流程是明白了，看看后边有没有机会实际用一下。
更多推荐
17
0- 0
已为社区贡献2条内容
所有评论(0)