Apollo camera 感知部分 目标检测源码阅读分析_solve3dbboxgivenonefullbboxdimensionorientation

障碍物检测分为detector transformer postprocessor tracker几个部分

kitti 基于单目的3D目标检测的预备知识点：

首先是yolo3d 3d目标检测输出结果为kitti的3D格式，（其中3d信息部分是以相机坐标系为参考坐标系的）

首先介绍下kitti 3d object detection障碍物标注的的标注文件格式

KITTI数据集，label文件解析：

Car 0.00 0 -1.84 662.20 185.85 690.21 205.03 1.48 1.36 3.51 5.35 2.56 58.84 -1.75

第1个字符串：代表物体类别

'Car', 'Van', 'Truck','Pedestrian', 'Person_sitting', 'Cyclist','Tram',  'Misc' or  'DontCare'

注意：’DontCare’ 标签表示该区域没有被标注，比如由于目标物体距离激光雷达太远。为了防止在评估过程中（主要是计算precision），将本来是目标物体但是因为某些原因而没有标注的区域统计为假阳性(false positives)，评估脚本会自动忽略’DontCare’ 区域的预测结果。

第2个数：代表物体是否被截断，从0（非截断）到1（截断）浮动，其中truncated指离开图像边界的对象

第3个数：代表物体是否被遮挡，整数0，1，2，3表示被遮挡的程度

0：完全可见  1：小部分遮挡  2：大部分遮挡 3：完全遮挡（unknown）

第4个数：alpha，物体的观察角度，范围：-pi~pi

是在相机坐标系下，以相机原点为中心，相机原点到物体中心的连线为半径，将物体绕相机y轴旋转至相机z轴，此时物体方向与相机x轴的夹角

图1

r_y + pi/2 -theta = alpha +pi/2（即图中紫色的角是相等的）

所以alpha = r_y – theta

第5～8这4个数：物体的2维边界框，左上角和右下角的像素坐标

第9～11这3个数：3维物体的尺寸，高、宽、长（单位：米）

第12～14这3个数：3维物体的位置 x,y,z（在照相机坐标系下，单位：米）

第15个数：3维物体的空间方向：rotation_y，在照相机坐标系下，相对于y轴的旋转角，范围：-pi~pi

物体前进方向与相机坐标系x轴的夹角，即上面的r_y

有些有第16个数：检测的置信度 ， 仅用于结果：浮点，p / r曲线所需，越高越好

3、KITTI数据集，calib解析

要将Velodyne坐标中的点x投影到左侧的彩色图像中y：

使用公式：y = P2 * R0_rect *Tr_velo_to_cam * x

将Velodyne坐标中的点投影到右侧的彩色图像中：

使用公式：y = P3 * R0_rect *Tr_velo_to_cam * x

Tr_velo_to_cam * x    ：是将Velodyne坐标中的点x投影到编号为0的相机（参考相机）坐标系中

R0_rect *Tr_velo_to_cam * x    ：是将Velodyne坐标中的点x投影到编号为0的相机（参考相机）坐标系中，再修正

P2 * R0_rect *Tr_velo_to_cam * x     ：是将Velodyne坐标中的点x投影到编号为0的相机（参考相机）坐标系中，再修正，然后投影到编号为2的相机（左彩色相机）

注意：所有矩阵都存储在主行中，即第一个值对应于第一行。 R0_rect包含一个3x3矩阵，需要将其扩展为4x4矩阵，方法是在右下角添加1，在其他位置添加0。Tr_xxx是一个3x4矩阵（R | t），需要以相同的方式扩展到4x4矩阵！

通过使用校准文件夹中的3x4投影矩阵，可以将相机坐标系中的坐标投影到图像中，对于提供图像的左侧彩色相机，必须使用P2。rotation_y和alpha之间的区别在于rotation_y直接在相机坐标中给出，而alpha也会考虑从相机中心到物体中心的矢量，以计算物体相对于相机的相对方向。 例如，沿着摄像机坐标系的X轴面向的汽车，无论它位于X / Z平面（鸟瞰图）中的哪个位置，它的rotation_y都为 0，而只有当此车位于相机的Z轴上时α才为零，当此车从Z轴移开时，观察角度α将会改变。

detection:

apollo 采用的yolo3d 基于caffe变种 没有开源，网上开源的方法有 deep3dbox 

查看kitti排名可以找出许多方法，例如groomed_nms kinematic3d RTM3D(该方法可以更快落地)

http://www.cvlibs.net/datasets/kitti/eval_object.php?obj_benchmark=3d

上图是我测试后RT3D的基于单目的3d目标检测效果示意图

apollo 的yolo是基于早期的deep3dbox原理做的，因此在上述label标注元素中介绍的只有alpha是通过网络直接获得的角度，关于rotation_y 和 theta_ray角度是根据坐标转换和相机外参求得的。

transformer:

主要是如下三个处理步骤：

 // set object mapper options
    float theta_ray = 0.0f;
    SetObjMapperOptions(obj, camera_k_matrix, width_image, height_image,
                        &obj_mapper_options, &theta_ray);
 
    // process
    mapper_->Solve3dBbox(obj_mapper_options, object_center, dimension_hwl,
                         &rotation_y);
 
    // fill back results
    FillResults(object_center, dimension_hwl, rotation_y, camera2world_pose,
                theta_ray, obj);

其中第一个是参数配置

第二个函数需要解析的一些重点操作如下：

apollo求取这些角度的方式如下代码，思路：获得图相框底边的中心点 image_point_low_center 根据内参矩阵（camera intrinsics）camera_k_matrix得到相机坐标系下的3D坐标点 point_in_camera,然后利用图1所示的原理获得 theta_ray rotation_y 两个方位角度信息。

//multicue_obstacle_transformer.cc
 
float box_cent_x = (bbox2d[0] + bbox2d[2]) / 2;
  Eigen::Vector3f image_point_low_center(box_cent_x, bbox2d[3], 1);
  Eigen::Vector3f point_in_camera =
      static_cast<Eigen::Matrix<float, 3, 1, 0, 3, 1>>(
          camera_k_matrix.inverse() * image_point_low_center);
  *theta_ray =
      static_cast<float>(atan2(point_in_camera.x(), point_in_camera.z()));
  float rotation_y =
      *theta_ray + static_cast<float>(obj->camera_supplement.alpha);

求取目标物在相机坐标系下的中心点坐标：

利用小孔成像原理：

ObjMapper::SolveCenterFromNearestVerticalEdge

该函数用于获取目标中心点在相机坐标系下的坐标，以下代码做了注释。

bool ObjMapper::SolveCenterFromNearestVerticalEdge(const float *bbox,
                                                   const float *hwl, float ry,
                                                   float *center,
                                                   float *center_2d) const {
  center[0] = center[1] = center[2] = 0.0f;
  float height_bbox = bbox[3] - bbox[1];
  float width_bbox = bbox[2] - bbox[0];
  if (width_bbox <= 0.0f || height_bbox <= 0.0f) {
    AERROR << "width or height of bbox is 0";
    return false;
  }
 
  if (common::IRound(bbox[3]) >= height_ - 1) {
    height_bbox /= params_.occ_ratio;
  }
//求取目标物距离相机近的一面的在相机坐标系下的深度
  float f = (k_mat_[0] + k_mat_[4]) / 2;
  float depth = f * hwl[0] * common::IRec(height_bbox);
 
  // compensate from the nearest vertical edge to center
  const float PI = common::Constant<float>::PI();
  float theta_bbox = static_cast<float>(atan(hwl[1] * common::IRec(hwl[2])));
  float radius_bbox =
      common::ISqrt(common::ISqr(hwl[2] / 2) + common::ISqr(hwl[1] / 2));
 
  float abs_ry = fabsf(ry);
  float theta_z = std::min(abs_ry, PI - abs_ry) + theta_bbox;
  theta_z = std::min(theta_z, PI - theta_z);
//求目标物中心点深度 需要加上目标物长度的一半并考虑角度来计算
  depth += static_cast<float>(fabs(radius_bbox * sin(theta_z)));
 
  // back-project to solve center 获得中心点坐标（kitti 3d label中的那几列，利用内参投影获得）
  center_2d[0] = (bbox[0] + bbox[2]) / 2;
  center_2d[1] = (bbox[1] + bbox[3]) / 2;
  if (hwl[1] > params_.stable_w) {
    GetCenter(bbox, depth, ry, hwl, center, center_2d);
  } else {
    center[2] = depth;
    UpdateCenterViaBackProjectZ(bbox, hwl, center_2d, center);
  }
 
  return center[2] > params_.depth_min;
}

bool ObjMapper::Solve3dBboxGivenOneFullBboxDimensionOrientation(
    const float *bbox, const float *hwl, float *ry, float *center) {
  const float PI = common::Constant<float>::PI();
  const float PI_HALF = PI / 2;
  const float small_angle_diff =
      common::IDegreeToRadians(params_.angle_resolution_degree);
  float center_2d[2] = {0};
  bool success =
      SolveCenterFromNearestVerticalEdge(bbox, hwl, *ry, center, center_2d);
  float min_x = static_cast<float>(params_.boundary_len);
  float min_y = static_cast<float>(params_.boundary_len);
  float max_x = static_cast<float>(width_ - params_.boundary_len);
  float max_y = static_cast<float>(height_ - params_.boundary_len);
//截断属性 判断是否截断
  bool truncated = bbox[0] <= min_x || bbox[2] >= max_x || bbox[1] <= min_y ||
                   bbox[3] >= max_y;
//判断是否超过考虑的距离 目标物过远过小
  float dist_rough = sqrtf(common::ISqr(center[0]) + common::ISqr(center[2]));
  bool ry_pred_is_not_reliable = dist_rough > params_.dist_far &&
                                 bbox[3] - bbox[1] < params_.small_bbox_height;
  if (ry_pred_is_not_reliable || std::abs(*ry - PI_HALF) < small_angle_diff ||
      std::abs(*ry + PI_HALF) < small_angle_diff) {
    *ry = *ry > 0.0f ? PI_HALF : -PI_HALF;
  }
  if (!truncated) {
    PostRefineOrientation(bbox, hwl, center, ry);
    success =
        SolveCenterFromNearestVerticalEdge(bbox, hwl, *ry, center, center_2d);
    PostRefineZ(bbox, hwl, center_2d, *ry, center);
  } else {
    FillRyScoreSingleBin(*ry);
  }
  return success &&
         GetProjectionScore(*ry, bbox, hwl, center, true) > params_.iou_suc;
}

获得相机坐标系下三维框的函数是：

bool ObjMapper::Solve3dBbox(const ObjMapperOptions &options, float center[3],
                            float hwl[3], float *ry)

第三个函数fillresults 是吧求得的所有结果 中心点 追踪id 长宽高 角度等内容存储起来：

// fill back results
FillResults(object_center, dimension_hwl, rotation_y, camera2world_pose,
            theta_ray, obj);

上述分析完成后 是代码移植，移植代码到windows vs2017的开发环境

模型文件所在位置：

apollo\modules\perception\production\data\perception\camera\models\yolo_obstacle_detector\3d-r4-half