百度apollo 7.0 感知代码分析_apollo 障碍物u010647296

主要参考：
自动驾驶 Apollo 源码分析系列，感知篇(七)：Lidar 障碍物检测基本流程

目录架构

|-- BUILD      // 基础类
|-- base       // 基础类
|-- camera     // 相机相关 
|-- common     // 公共目录
|-- data       // 相机的内参和外参
|-- fusion     // 传感器融合   
|-- inference  // 深度学习推理模块
|-- lib        // 一些基础的库，包括线程、文件配置等
|-- lidar      // 激光雷达相关
|-- map        // 地图
|-- onboard    // 各个子模块的入口
|-- production // 感知模块入口 --- 通过cyber启动子模块
|-- proto      // 数据格式，protobuf
|-- radar      // 毫米波雷达
|-- testdata   // 几个模块的测试数据
`-- tool       // 离线测试工具
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16

其中重点关注- onboard和 production
 onboard 为各个子模块的入口，比如说感知就是在onboard/component/detection.cc 中

production 中的各个模块则包含启动这些模块用的dag文件，launch文件，还有各种配置文件。配置文件可以先看conf中，然后再去data中。

比如：
激光雷达感知：
 Apollo/modules/perception/production/conf/perception/lidar/velodyne128_detection_conf.pb.txt：
 LidarObstacleDetection在文件中是存储在 apollo/modules/perception/production/conf/perception/lidar/modules/lidar_obstacle_pipeline.config

pollo/modules/perception/production/data/perception/lidar/models/lidar_obstacle_pipeline/velodyne128/lidar_obstacle_detection.conf

这一部分内容参考[Apollo 7.0——percception：lidar源码剖析（万字长文）](https://zhuanlan.zhihu.com/p/542298434?utm_id=0) 
1
2
3
4
5
6
7
8
9
10
11
12
13

第一阶段：

要创建并启动一个算法组件, 需要通过以下4个步骤:

初如化组件的文件结构
实现组件类设置
配置文件 
启动组件
1
2
3
4

一个 component 需要创建以下文件：
    Header file: common_component_example.h
    Source file: http://common_component_example.cc
   Build file: BUILDDAG 
   dependency file: common.dag
   Launch file: common.launch

在component.cc中可以看到先执行Init()，再执行Proc(),这是cyber的特性
1
2
3
4
5
6
7
8

先来到：可以看到：
1

DetectionComponent::Init()：加载参数，其实这些都不重要，只要知道最后的处理函数在那里就可以了。
最后找到这个位置：velodyne128_detection_conf.pb.txt

sensor_name: "velodyne128"
detector_name: "LidarObstacleDetection"
enable_hdmap: true  //高精度地图
lidar_query_tf_offset: 0
lidar2novatel_tf2_child_frame_id: "velodyne128" //frameID
output_channel_name: "/perception/inner/DetectionObjects" //输出的名字 topic的名字？
1
2
3
4
5
6

然后在后面的DetectionComponent::InitAlgorithmPlugin()方法中，做了两件事情，第一件是：

//第一件：  detector_.reset(detector); 创建了detector对象，后面都是围绕这个展开的 
  lidar::BaseLidarObstacleDetection* detector =
      lidar::BaseLidarObstacleDetectionRegisterer::
      GetInstanceByName(detector_name_);
  CHECK_NOTNULL(detector);
  detector_.reset(detector);
//第二件：坐标转换
  lidar2world_trans_.Init(lidar2novatel_tf2_child_frame_id_);
1
2
3
4
5
6
7
8

接下来来到了Proc方法，这里调用了InternalProc(message, out_message);//3

bool DetectionComponent::Proc(
    const std::shared_ptr<drivers::PointCloud>& message) {
  AINFO << std::setprecision(16)
        << "Enter detection component, message timestamp: "
        << message->measurement_time()
        << " current timestamp: " << Clock::NowInSeconds();

  auto out_message = std::make_shared<LidarFrameMessage>();

  bool status = InternalProc(message, out_message);//3
  if (status) {
    writer_->Write(out_message);
    AINFO << "Send lidar detect output message.";
  }
  return status;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16

InternalProc(message, out_message);中最关键的一句就是：

  lidar::LidarProcessResult ret =
      detector_->Process(detect_opts, in_message, frame.get());//4
1
2

到此第一阶段结束，后面看detector_->Process(detect_opts, in_message, frame.get());怎么处理的点云。

第二阶段

文件的位置在这里：modules/perception/lidar/app/lidar_obstacle_detection.cc

LidarProcessResult LidarObstacleDetection::Process(// 4 主要是这个函数
    const LidarObstacleDetectionOptions& options,
    const std::shared_ptr<apollo::drivers::PointCloud const>& message,
    LidarFrame* frame) {
  const auto& sensor_name = options.sensor_name;

  PERF_FUNCTION_WITH_INDICATOR(options.sensor_name);

  PERF_BLOCK_START();
  PointCloudPreprocessorOptions preprocessor_options;
  preprocessor_options.sensor2novatel_extrinsics =
      options.sensor2novatel_extrinsics;
  PERF_BLOCK_END_WITH_INDICATOR(sensor_name, "preprocess");
  //这里：
  if (cloud_preprocessor_->Preprocess(preprocessor_options, message, frame)) {
    return ProcessCommon(options, frame);
  }
  return LidarProcessResult(LidarErrorCode::PointCloudPreprocessorError,
                            "Failed to preprocess point cloud.");
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20

1. 点云预处理：Preprocesses() modules/perception/lidar/lib/pointcloud_preprocessor/pointcloud_preprocessor.cc
2. 点云检测： ProcessCommon()
   其中点云预处理Preprocesses() 比较简单，就是将 PointCloud 中的点云过滤，然后复制到 LidarFrame 结构体对应的 cloud 中。
   **1 Preprocesses() ： **

2 ProcessCommon()

会发现 detector，代码调用了其 Detect() 方法，在modules/perception/lidar/app/lidar_obstacle_detection.cc
找到bool LidarObstacleDetection::Init(）函数，发现了
在apollo readme.txt中我们可以看到
启动
为了方便Apollo模型的拓展，我们重构了检测模块以允许更多检测模型可以便捷的添加和切换。只需要修改对应的配置文件就可以选择启动不同的模型。相关的配置文件在路径下：

modules/perception/production/data/perception/lidar/models/lidar_obstacle_pipeline/


1
2
3

下面有多个目录，对应着不同的设备名称。对于激光雷达传感器，修改目录下的"lidar_obstacle_detection.conf"配置文件的detector关键字即可切换检测模型。我们可以找到：

另外：detector 的类用虚函数定义了接口，在lidar障碍物检测基类对象指针指向子类的对象，对应代码：

lidar::BaseLidarObstacleDetection *detector = lidar::BaseLidarObstacleDetectionRegisterer::GetInstanceByName(detector_name_);     // 调用宏定义类的静态方法
1

多态实现，这里没有看太明白，知道在那里找到，modules/perception/lidar/app/lidar_obstacle_detection.cc中:

class LidarObstacleDetection : public BaseLidarObstacleDetection,
这样就跳转到lidar_obstacle_detection.cc中，即detector 为lidar_obstacle_detection
1
2

BaseLidarObstacleDetectionRegistere作为雷达障碍物检测的基类，通过多态形式调用子类的init函数，路径：

apollo/modules/perception/lidar/lib/interface/base_lidar_obstacle_detection.h

lidar::BaseLidarObstacleDetectionRegisterer调用一个宏定义类，类BaseLidarObstacleDetectionRegisterer路径：apollo/modules/perception/lib/registerer/registerer.h

#define PERCEPTION_REGISTER_REGISTERER(base_class)                    \
  class base_class##Registerer {                                      \
    typedef ::apollo::perception::lib::Any Any;                       \
    typedef ::apollo::perception::lib::FactoryMap FactoryMap;         \
                                                                      \
   public:                                                            \
    static base_class *GetInstanceByName(const ::std::string &name) { \
      FactoryMap &map =                                               \
          ::apollo::perception::lib::GlobalFactoryMap()[#base_class]; \
      FactoryMap::iterator iter = map.find(name);                     \
      if (iter == map.end()) {                                        \
        for (auto c : map) {                                          \
          AERROR << "Instance:" << c.first;                           \
        }                                                             \
        AERROR << "Get instance " << name << " failed.";              \
        return nullptr;                                               \
      }                                                               \
      Any object = iter->second->NewInstance();                       \
      return *(object.AnyCast<base_class *>());                       \
    }                                                                 \
    static std::vector<base_class *> GetAllInstances() {              \
      std::vector<base_class *> instances;                            \
      FactoryMap &map =                                               \
          ::apollo::perception::lib::GlobalFactoryMap()[#base_class]; \
      instances.reserve(map.size());                                  \
      for (auto item : map) {                                         \
        Any object = item.second->NewInstance();                      \
        instances.push_back(*(object.AnyCast<base_class *>()));       \
      }                                                               \
      return instances;                                               \
    }                                                                 \
    static const ::std::string GetUniqInstanceName() {                \
      FactoryMap &map =                                               \
          ::apollo::perception::lib::GlobalFactoryMap()[#base_class]; \
      CHECK_EQ(map.size(), 1U) << map.size();                         \
      return map.begin()->first;                                      \
    }                                                                 \
    static base_class *GetUniqInstance() {                            \
      FactoryMap &map =                                               \
          ::apollo::perception::lib::GlobalFactoryMap()[#base_class]; \
      CHECK_EQ(map.size(), 1U) << map.size();                         \
      Any object = map.begin()->second->NewInstance();                \
      return *(object.AnyCast<base_class *>());                       \
    }                                                                 \
    static bool IsValid(const ::std::string &name) {                  \
      FactoryMap &map =                                               \
          ::apollo::perception::lib::GlobalFactoryMap()[#base_class]; \
      return map.find(name) != map.end();                             \
    }                                                                 \
  };

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57

第三阶段

这一阶段主要是为了分析用到piontpillar的检测全过程，对应代码的位置在：modules/perception/lidar/lib/detection/lidar_point_pillars/point_pillars_detection.cc

把整个代码贴上来：

bool PointPillarsDetection::Detect(const LidarDetectorOptions& options,//8
                                   LidarFrame* frame) {
  // check input
  if (frame == nullptr) {
    AERROR << "Input null frame ptr.";
    return false;
  }
  if (frame->cloud == nullptr) {
    AERROR << "Input null frame cloud.";
    return false;
  }
  if (frame->cloud->size() == 0) {
    AERROR << "Input none points.";
    return false;
  }

  // record input cloud and lidar frame
  original_cloud_ = frame->cloud;
  original_world_cloud_ = frame->world_cloud;
  lidar_frame_ref_ = frame;

  // check output
  frame->segmented_objects.clear();

  if (cudaSetDevice(FLAGS_gpu_id) != cudaSuccess) {
    AERROR << "Failed to set device to gpu " << FLAGS_gpu_id;
    return false;
  }

  Timer timer;

  int num_points;
  cur_cloud_ptr_ = std::shared_ptr<base::PointFCloud>(
      new base::PointFCloud(*original_cloud_));

  // down sample the point cloud through filtering beams
  if (FLAGS_enable_downsample_beams) {
    base::PointFCloudPtr downsample_beams_cloud_ptr(new base::PointFCloud());
    if (DownSamplePointCloudBeams(original_cloud_, downsample_beams_cloud_ptr,//第一次下采样
                                  FLAGS_downsample_beams_factor)) {
      cur_cloud_ptr_ = downsample_beams_cloud_ptr;
    } else {
      AWARN << "Down-sample beams factor must be >= 1. Cancel down-sampling."
               " Current factor: "
            << FLAGS_downsample_beams_factor;
    }
  }

  // down sample the point cloud through filtering voxel grid
  if (FLAGS_enable_downsample_pointcloud) {
    pcl::PointCloud<pcl::PointXYZI>::Ptr pcl_cloud_ptr(
        new pcl::PointCloud<pcl::PointXYZI>());
    pcl::PointCloud<pcl::PointXYZI>::Ptr filtered_cloud_ptr(
        new pcl::PointCloud<pcl::PointXYZI>());
    TransformToPCLXYZI(*cur_cloud_ptr_, pcl_cloud_ptr);
    DownSampleCloudByVoxelGrid(第二次下采样
        pcl_cloud_ptr, filtered_cloud_ptr, FLAGS_downsample_voxel_size_x,
        FLAGS_downsample_voxel_size_y, FLAGS_downsample_voxel_size_z);

    // transform pcl point cloud to apollo point cloud
    base::PointFCloudPtr downsample_voxel_cloud_ptr(new base::PointFCloud());
    TransformFromPCLXYZI(filtered_cloud_ptr, downsample_voxel_cloud_ptr);
    cur_cloud_ptr_ = downsample_voxel_cloud_ptr;
  }
  downsample_time_ = timer.toc(true);

  num_points = cur_cloud_ptr_->size();
  AINFO << "num points before fusing: " << num_points;

  // fuse clouds of preceding frames with current cloud
  cur_cloud_ptr_->mutable_points_timestamp()->assign(cur_cloud_ptr_->size(),
                                                     0.0);
  if (FLAGS_enable_fuse_frames && FLAGS_num_fuse_frames > 1) {
    // before fusing
    while (!prev_world_clouds_.empty() &&
           frame->timestamp - prev_world_clouds_.front()->get_timestamp() >
               FLAGS_fuse_time_interval) {
      prev_world_clouds_.pop_front();
    }
    // transform current cloud to world coordinate and save to a new ptr
    base::PointDCloudPtr cur_world_cloud_ptr =
        std::make_shared<base::PointDCloud>();
    for (size_t i = 0; i < cur_cloud_ptr_->size(); ++i) {
      auto& pt = cur_cloud_ptr_->at(i);
      Eigen::Vector3d trans_point(pt.x, pt.y, pt.z);
      trans_point = lidar_frame_ref_->lidar2world_pose * trans_point;
      PointD world_point;
      world_point.x = trans_point(0);
      world_point.y = trans_point(1);
      world_point.z = trans_point(2);
      world_point.intensity = pt.intensity;
      cur_world_cloud_ptr->push_back(world_point);
    }
    cur_world_cloud_ptr->set_timestamp(frame->timestamp);

    // fusing clouds
    for (auto& prev_world_cloud_ptr : prev_world_clouds_) {
      num_points += prev_world_cloud_ptr->size();
    }
    FuseCloud(cur_cloud_ptr_, prev_world_clouds_);

    // after fusing
    while (static_cast<int>(prev_world_clouds_.size()) >=
           FLAGS_num_fuse_frames - 1) {
      prev_world_clouds_.pop_front();
    }
    prev_world_clouds_.emplace_back(cur_world_cloud_ptr);
  }
  AINFO << "num points after fusing: " << num_points;
  fuse_time_ = timer.toc(true);

  // shuffle points and cut off
  if (FLAGS_enable_shuffle_points) {
    num_points = std::min(num_points, FLAGS_max_num_points);
    std::vector<int> point_indices = GenerateIndices(0, num_points, true);
    base::PointFCloudPtr shuffle_cloud_ptr(
        new base::PointFCloud(*cur_cloud_ptr_, point_indices));
    cur_cloud_ptr_ = shuffle_cloud_ptr;
  }
  shuffle_time_ = timer.toc(true);

  // point cloud to array
  float* points_array = new float[num_points * FLAGS_num_point_feature]();
  CloudToArray(cur_cloud_ptr_, points_array, FLAGS_normalizing_factor);
  cloud_to_array_time_ = timer.toc(true);

  // inference
  std::vector<float> out_detections;
  std::vector<int> out_labels;
  point_pillars_ptr_->DoInference(points_array, num_points, &out_detections,
                                  &out_labels);
  inference_time_ = timer.toc(true);

  // transfer output bounding boxes to objects
  GetObjects(&frame->segmented_objects, frame->lidar2world_pose,
             &out_detections, &out_labels);
  collect_time_ = timer.toc(true);

  AINFO << "PointPillars: "
        << "\n"
        << "down sample: " << downsample_time_ << "\t"
        << "fuse: " << fuse_time_ << "\t"
        << "shuffle: " << shuffle_time_ << "\t"
        << "cloud_to_array: " << cloud_to_array_time_ << "\t"
        << "inference: " << inference_time_ << "\t"
        << "collect: " << collect_time_;

  delete[] points_array;
  return true;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150

点云送到模型检测前要经过 downsample 步骤。
downsample 还经过 2 次：

DownSamplePointCloudBeams
DownSampleByVoxelGrid
1
2

DownSamplePointCloudBeams( )在这里 downsample_factor 是大于 1 的整数，是下采样的因子，有点像卷积操作中的 stride，每隔 downsample_factor 取一点，最终减少了总体点云数据量。

fuse 的是当前的点云和之前的点云。

把过滤掉无效点云的历史数据全部加入当前点云当中。
问题：为什么点云需要前后数据的融合呢？
数据融合之后，再做一些 shuffle 之类的操作就直接送到推断引擎中去做前向推断了，最后又通过神经网络输出的结果得到最终的检测目标。
看一下DoInference怎么定义的：

另外，可以看到 模型位置在：/modules/perception/production/data/perception/lidar/models/detection/point_pillars

整个过程就到这里了，后面看有没有机会把这部分代码移植出来，看看效果。