1. 问题描述

BEV 地面点表示为：

$$ P_g = \begin{bmatrix} x \\ y \\ 1 \end{bmatrix} $$

通过地面到相机的变换矩阵得到相机坐标：

$$ P_c = H_{c\_g}P_g = \begin{bmatrix} X_c \\ Y_c \\ Z_c \end{bmatrix} $$

对于 $y \rightarrow 0$ 或 $y < 0$ 的近处地面点，$Z_c$ 可能过小或为负数。直接进行鱼眼投影时，畸变半径可能变成负数，使投影方向翻转，导致本应位于图像底部下方的点错误地出现在图像顶部。

因此，需要适当修改输入的 xy.y。

2. 修正目标

修正后的 fixed_y 需要满足：

  1. 相机深度有效：

    $$ Z_c \geq minDepth $$

  2. 鱼眼畸变半径有效：

    $$ r_d(\theta) > 0 $$

  3. 投影点位于图像底部之外：

    $$ v \approx imageHeight + 1 $$

  4. fixed_y 尽可能接近原始输入：

    $$ \min |fixedY-xy.y| $$

因此，问题可以表示为：

$$ \min_{y'} |y'-xy.y| $$

约束条件为：

$$ SpaceToPlane\left(H_{c\_g}[x,y',1]^T\right).v=targetV $$

其中：

$$ targetV=imageHeight+1 $$

3. 判断是否需要修正

首先使用原始 xy.y 计算相机坐标：

const cv::Vec3d original_xyz_c =
    H_c_g * cv::Vec3d(xy.x, xy.y, 1.0);

如果同时满足以下条件，则保留原始 y：

  • $Z_c > minDepth$
  • 鱼眼畸变半径为正
if (depth_is_valid &&
    !has_folded_fisheye_radius(original_xyz_c)) {
  return xy.y;
}

这样可以避免修改能够正常投影的 BEV 点。

4. 深度约束

相机深度可以写成：

$$ Z_c=h_{20}x+h_{21}y+h_{22} $$

固定 $x$ 后，令：

$$ Z_c=minDepth $$

可以得到：

$$ y_{depth}

\frac{minDepth-h_{20}x-h_{22}}{h_{21}} $$

LimitBEVyByDepth() 使用该关系获得满足最小深度要求的搜索起点：

const double start_y =
    LimitBEVyByDepth(H_c_g, xy, min_depth);

5. 鱼眼畸变半径检查

鱼眼模型首先计算入射角：

$$ \theta= \arccos\left( \frac{Z_c} {\sqrt{X_c^2+Y_c^2+Z_c^2}} \right) $$

畸变后的半径为：

$$ r_d(\theta)= \theta +k_1\theta^3 +k_2\theta^5 +k_3\theta^7 +k_4\theta^9 $$

当：

$$ r_d(\theta)\leq 0 $$

时，投影方向会发生翻转。此时即使 $Z_c > minDepth$，投影结果仍可能错误地出现在图像顶部。

因此，候选点必须满足：

distorted_radius > 0.0

6. 确定搜索方向

由于：

$$ Z_c=h_{20}x+h_{21}y+h_{22} $$

所以 $h_{21}$ 决定增加相机深度时应该沿哪个方向修改 y：

const double direction = h21 > 0.0 ? 1.0 : -1.0;
  • 当 $h_{21}>0$ 时，增大 y
  • 当 $h_{21}<0$ 时，减小 y

7. 搜索距离输入最近的交点

目标像素行设置为：

const double target_v =
    static_cast<double>(img_size_.height) + 1.0;

选择 $imageHeight+1$ 是为了让投影点刚好位于图像底部之外，从而尽量减小对 xy.y 的修改。

从 start_y 开始，沿深度增加方向使用固定步长搜索：

constexpr double kScanStep = 0.01;
constexpr int kMaxScanSteps = 5000;

第 $i$ 个候选位置为：

$$ y_i=startY+direction\cdot i\cdot scanStep $$

因为搜索从距离输入最近的位置开始，所以遇到的第一个 target_v 交点，就是当前搜索方向上距离原始 xy.y 最近的解。

8. 判断是否跨越目标像素行

设相邻两个有效投影结果满足：

$$ v_{previous}以及：

$$ v_{current}\geq targetV $$

则说明对应的 y 区间内存在一个满足以下条件的交点：

$$ v(y)=targetV $$

代码中的判断为：

const bool crossed_target =
    std::isfinite(current_v) &&
    current_v >= target_v &&
    (!std::isfinite(previous_v) ||
     previous_v < target_v);

9. 二分法精确求解

找到交点区间后，通过二分法求解：

for (int j = 0; j < 50; ++j) {
  const double mid_y =
      0.5 * (lower_y + upper_y);

  const double mid_v =
      project_valid_v(mid_y);

  if (std::isfinite(mid_v) &&
      mid_v >= target_v) {
    upper_y = mid_y;
  } else {
    lower_y = mid_y;
  }
}

最终返回：

return upper_y;

此时满足：

$$ v(upperY)\approx targetV $$

并且该结果位于搜索到的第一个交点附近，因此对输入 xy.y 的修正量较小。

10. target_v 不可达时的处理

横向距离较大的射线可能从图像左右边界离开，不一定存在满足以下条件的交点：

$$ v=targetV $$

这种情况下，无法同时满足：

  • 达到 target_v
  • 保持固定的 x
  • 维持有效的鱼眼投影

因此回退到距离输入 xy.y 最近的合法候选点：

return found_valid_y
           ? nearest_valid_y
           : start_y;

该回退策略优先保证：

  • 相机深度有效
  • 鱼眼畸变半径未翻转
  • fixed_y 尽可能接近输入 xy.y

11. 完整流程

输入 BEV 点 (x, y)
        │
        ▼
转换到相机坐标 (Xc, Yc, Zc)
        │
        ▼
深度有效且鱼眼半径为正？
   ┌────┴────┐
   │         │
  是         否
   │         │
   ▼         ▼
返回原始 y   计算最小深度对应的 start_y
             │
             ▼
       确定深度增加方向
             │
             ▼
       从最近位置开始扫描
             │
             ▼
       找到第一个 target_v 交点？
        ┌────┴────┐
        │         │
       是         否
        │         │
        ▼         ▼
     二分求解     返回距离输入最近的
     fixed_y      合法候选点

12. 核心原则

该方法不是简单地强制：

$$ v\geq 0 $$

而是：

  • 防止鱼眼畸变半径变负造成投影方向翻转；
  • 让近处地面点沿正确方向落到图像底部之外；
  • 在满足投影有效性的前提下，使修正后的 y 尽可能接近输入的 xy.y。