月度归档： 2022年3月

Apollo参考线优化之DiscretePointsReferenceLineSmoother

原创文章，转载请注明： 转载自慢慢的回味

本文链接地址: Apollo参考线优化之DiscretePointsReferenceLineSmoother

Apollo的的规划算法基于Frenet坐标系，因此道路中心线的平滑性控制着车辆是否左右频繁晃动，而高精地图的道路中心线往往不够规划。Apollo在/modules/planning/reference_line中包含了多种参考线平滑算法：DiscretePointsReferenceLineSmoother（离散点平滑法，包括FEM_POS_DEVIATION_SMOOTHING有限元位置差异和COS_THETA_SMOOTHING余弦），QpSplineReferenceLineSmoother（三次样条插值法），SpiralReferenceLineSmoother（螺旋曲线法）。本篇以单元测试discrete_points_reference_line_smoother_test.cc的测试TEST_F(DiscretePointsReferenceLineSmootherTest, smooth)为例来分析Apollo对参考线reference line进行离散点平滑（FEM_POS_DEVIATION_SMOOTHING）的原理。

Content:
  1. 离散点平滑法原理
  2. 离散点平滑法测试用例
  3. 离散点平滑方法
  4. FemPosDeviationOsqpInterface求解器
  5. P矩阵
  6. q向量
  7. 设置限制并使用OSQP优化
离散点平滑法原理

Apollo默认采用的平滑算法，其将参考线平滑构造成了一个二次优化问题，并使用osqp求解器进行求解。那么通过构建它的代价函数及约束条件就可以利用二次优化框架直接求解。
1 首先在参考线上隔相同距离打点（P_k(x_k,y_k)），绿色的曲线就是算法将要得到的理想曲线。

2 然后列出代价函数：

    \[ cost=cost_{smooth}+cost_{length}+cost_{deviation} \]

其中，cost_{smooth}为平滑度代价，cost_{length}为长度代价，cost_{deviation}为相对原始点偏离代价。

    \[ cost_{smooth}=\sum_{k=0}^{n-3}\parallel (x_k+x_{k+2})-2x_{k+1} \parallel^2_2 \]

    \[ cost_{length}=\sum_{k=0}^{n-2}\parallel y_{k+1}-y_k \parallel^2_2 \]

    \[ cost_{deviation}=\sum_{k=0}^{n-1}\parallel z_k-z_{k-ref} \parallel^2_2 \]

cost_{smooth}要求相邻的3点尽量在同一条直线上，cost_{length}要求相邻2点不能太长，cost_{deviation}要求曲线上的点不能离参考点太远。在FEM_POS_DEVIATION_SMOOTHING算法中，cost_{smooth}的权重远远大于其它2个。

继续阅读“Apollo参考线优化之DiscretePointsReferenceLineSmoother”本作品采用知识共享署名 4.0 国际许可协议进行许可。

Apollo自动驾驶车道检测

原创文章，转载请注明： 转载自慢慢的回味

本文链接地址: Apollo自动驾驶车道检测

本篇以单元测试camera_lib_lane_postprocessor_darkscnn_lane_postprocessor_test.cc的测试camera_lane_postprocessor_point_test为例来分析Apollo自动驾驶怎么进行车道检测。
利用DarkSCNN算法对摄像头拍摄到的路面图片进行预测，来获取车道线在以车辆坐标系下的位置。

摄像头输出图片

DarkSCNN输出的车道线Mask图

融合到原图上的车道线

Content:
  1. DarkSCNN车道检测测试用例
  2. 车道检测器初始化
  3. RTNet网络加载
  4. SCNN车道检测主函数
  5. 2D后续处理

继续阅读“Apollo自动驾驶车道检测”本作品采用知识共享署名 4.0 国际许可协议进行许可。

Apollo自动驾驶的点云CNN分割

原创文章，转载请注明： 转载自慢慢的回味

本文链接地址: Apollo自动驾驶的点云CNN分割

点云分割的主要方法为：1、映射3D点云到2D的索引图上，对每个索引图上的像素点里面的点云进行高度，强度，数量的统计；2、对2D索引图进行CNN推断，从而得到每个像素点倾向的中心点，方向，所属类型的概率等；3、通过SPP引擎集群2D索引图上的像素点；4、对每个集群里的每个像素点求所属类型概率的平均值，从而得到集群所属的类型，点云的范围，中心点，高度等。从而完成了物体的识别。

这儿以单元测试cnn_segmentation_test.cc的测试cnn_segmentation_sequence_test为例来分析Apollo自动驾驶的点云CNN分割。

Content:
  1. CNN分割测试用例
  2. CNN分割初始化
  3. 侦测当前帧中的对象
  4. 特征提取器提取点云特征信息
  5. 处理CNN模型推测的集群信息
  6. 处理前景分割对象
  7. 调用并查集算法构建节点，合并节点
  8. SPP索引图处理
  9. 打印侦测到的对象信息

CNN分割测试用例
TEST(CNNSegmentationTest, cnn_segmentation_sequence_test) {
  unsetenv("CYBER_PATH");
  unsetenv("MODULE_PATH");
//设置工作目录和配置文件路经
  FLAGS_work_root =
      "/apollo/modules/perception/testdata/"
      "lidar/lib/segmentation/cnnseg/";
  FLAGS_config_manager_path = "../../../../../production/conf";
//实例化一个CNN分割类实例
  auto segmentation = std::shared_ptr<cnnsegmentation>(new CNNSegmentation);
  LidarDetectorOptions options;
  EXPECT_FALSE(segmentation->Detect(options, nullptr));
  LidarFrame frame_data;
  EXPECT_FALSE(segmentation->Detect(options, &frame_data));
  frame_data.cloud = base::PointFCloudPool::Instance().Get();
  frame_data.world_cloud = base::PointDCloudPool::Instance().Get();
  EXPECT_FALSE(segmentation->Detect(options, &frame_data));
 
//初始化CNN分割实例，包括参数加载，特征提取初始化，CNN推测初始化。见下面详细注解。
  EXPECT_TRUE(segmentation->Init());
  EXPECT_TRUE(segmentation->InitClusterAndBackgroundSegmentation());
 
  std::string pcd_path =
      "/apollo/modules/perception/testdata/lidar/app/data/";
  std::vector<std::string> pcd_file_names;
  common::GetFileList(pcd_path, ".pcd", &pcd_file_names);
  std::string file_name;
  std::sort(pcd_file_names.begin(), pcd_file_names.end(),
            [](const std::string& lhs, const std::string& rhs) {
              if (lhs.length() < rhs.length()) {
                return true;
              } else if (lhs.length() == rhs.length()) {
                return lhs <= rhs;
              } else {
                return false;
              }
            });
  for (size_t i = 0; i < pcd_file_names.size(); ++i) {
    std::shared_ptr<lidarframe> frame(new LidarFrame);
    frame->cloud = base::PointFCloudPool::Instance().Get();
    frame->world_cloud = base::PointDCloudPool::Instance().Get();
//加载点云文件，比如这儿为/apollo/modules/perception/testdata/lidar/app/data/0001_00.pcd，可视化的结果如下图
    if (!LoadPCDFile(pcd_file_names[i], frame->cloud)) {
      continue;
    }
    frame->world_cloud->resize(frame->cloud->size());
//侦测当前帧中的对象，见下面代码详解
    EXPECT_TRUE(segmentation->Detect(options, frame.get()));
//打印侦测到的对象信息，见下面代码详解
    PrintObjects((frame.get())->segmented_objects);
  }
}
</lidarframe></std::string></cnnsegmentation>

测试用例PCD文件可视化结果
继续阅读“Apollo自动驾驶的点云CNN分割”本作品采用知识共享署名 4.0 国际许可协议进行许可。