无 OpenCV 对图像逆透视

前言

伴随着无人驾驶技术的发展，越来越多人接触到了更新的科技，也越来越好奇计算机世界到底是如何实现自动驾驶，而自动驾驶系统附带的某些功能又是如何实现的也引起了很多人的好奇。这次将讲述并且实践系统中 「360° 倒车影像」背后的算法逻辑。

逆透视变换

在画面获取时，整车会调用多个摄像头，拼成一幅「360°全景照片」

而形成俯视视角的 「360° 倒车影像」 要经过一层数学运算，也就是逆透视变换，简称IPM。

在这个领域，有很多种IPM变换方式，比如「对应点对单应变换方法」、「简化相机模型逆透视变换」，但都利用了矩阵的变换法则。

对应点对单应变换方法

这个变化方式相对简单，不过多描述。

输入至少四个对应点对，不能有三点及以上共线，不需要知道摄相机参数或者平面位置的任何信息，利用点对，求解透视变换矩阵，其中矩阵是一个三阶方阵，因此可以构建一个线性方程求解。如果大于四个点，可以使用 $ransac$ 方法求解，而选点的方法通常采用手动选取，一般是选择消失点。

\begin{bmatrix} t_i x'_i \\ t_i y'_i \\ t_i \end{bmatrix} = map\_matrix \cdot \begin{bmatrix} x_i \\ y_i \\ 1 \end{bmatrix}

dst(i) = (x'_i, y'_i), src(i) = (x_i, y_i), i = 0, 1, 2, 3

这个变化在代码上实现相对简单，可以相对容易实现 IPM 变换，这里就不再过多赘述，也不提供代码示例。

简化相机模型 IPM方法

这是这次重点要解析的变换方法，这个算法的本质是利用相机成像过程中的各种坐标之间的转换关系，再对它进行抽象和简化，最终得到的世界坐标。

然后再建立世界坐标与图像坐标的对应关系，使用该关系进行数学变换得来。

不同于某些复杂、冗长的计算公式，这里我们依旧采用坐标运算。针对这种 IPM 计算方法，我们需要先测量摄像头实际参数。

这里仰角 $θ$ 为 $23°$ ，中心高度 $H$ 为 $37 cm$ ，视点到视平面距离 $d$ 为 $87 cm$ ，然后求世界坐标 $P_W$ 。

设摄像头图像坐标为 $\boldsymbol{P_G} = (x, y, z, 1)$ ，并由世界坐标与图像坐标的关系建立矩阵方程，

\boldsymbol{P_G} = \boldsymbol{P_W} \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & -H & 1 \end{bmatrix} \begin{bmatrix} \cos\theta & 0 & -\sin\theta & 0 \\ 0 & 1 & 0 & 0 \\ \sin\theta & 0 & \cos\theta & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \cdot \begin{bmatrix} 1 & 0 & 0 & \frac{1}{d} \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}. \tag{1}

将图像坐标代入式$(1)$，求得世界坐标的矩阵，即

\boldsymbol{P_W} = \left[ \frac{x\cos\theta - z\sin\theta}{1 - \frac{x}{d}} \quad \frac{y}{1 - \frac{x}{d}} \quad \frac{x\sin\theta + z\cos\theta - \frac{H}{d}}{1 - \frac{x}{d}} \quad 1 \right]. \tag{2}

令 $A = H\cos\theta$ 以及 $B = -d$ 还有 $C = d\sin\theta - \frac{H}{d}$ 以及 $D = \cos\theta$ 还有 $E = d\sin\theta$ ，由几何关系可知 $x\sin\theta + z\cos\theta - \frac{H}{d} = 0$ 。求得 $\boldsymbol{P_W}$ 最简形式为

\boldsymbol{P_W} = \left[ \frac{A + Bz}{C + Dz} \quad \frac{Ey}{C + Dz} \quad 0 \quad 1 \right]. \tag{3}

最后，处理图像。由于所处理的图像是二位平面图，故图像深度恒为0。根据$(3)$，只需要把数组的横纵坐标带入，即可求得世界坐标下坐标值，即 IPM 后的俯视图。

#include <cmath>
#include <cstdint>
#include <vector>
#include <algorithm>

namespace ipm
{
    // =========================
    // 基础数据结构
    // =========================

    struct Vec3
    {
        double x;
        double y;
        double z;
    };

    struct GroundPoint
    {
        double X;     // 世界坐标 X（左右）
        double Y;     // 世界坐标 Y（前后）
        bool valid;   // 是否与地面有有效交点
    };

    struct CameraParam
    {
        // 焦距（像素单位）
        // 如果你只有一个 d，可以令 fx = fy = d
        double fx;
        double fy;

        // 主点（通常是图像中心）
        double cx;
        double cy;

        // 相机离地高度，单位例如 cm
        double H;

        // 相机向下俯角（弧度）
        double pitch;
    };

    struct IPMParam
    {
        // 输出俯视图尺寸
        int outWidth;
        int outHeight;

        // 世界坐标范围（单位与 H 一致，例如 cm）
        // X: 左右范围
        // Y: 前后范围
        double minX;
        double maxX;
        double minY;
        double maxY;
    };

    // =========================
    // 工具函数
    // =========================

    inline double clampDouble(double v, double lo, double hi)
    {
        return (v < lo) ? lo : ((v > hi) ? hi : v);
    }

    inline uint8_t clampToByte(double v)
    {
        if (v < 0.0) return 0;
        if (v > 255.0) return 255;
        return static_cast<uint8_t>(v + 0.5);
    }

    // 绕 X 轴旋转：把相机坐标系下的方向，转到世界坐标系
    // 这里假定：
    // - 世界 Z 轴向上
    // - 相机光轴默认朝世界 Y 正方向
    // - pitch > 0 表示相机向下俯视
    //
    // 为了和图像坐标（v向下）匹配，构造一个工程上常用的映射：
    //
    // 相机系射线 rc = [x, y, 1]
    // 先映射到“未俯仰时”的世界方向：
    //   x -> Xw
    //   y -> -Zw
    //   z -> Yw
    //
    // 再绕世界 X 轴旋转 pitch
    //
    inline Vec3 cameraRayToWorldRay(const Vec3& rc, double pitch)
    {
        // 未俯仰时的世界方向
        // 相机右 -> 世界右
        // 相机下 -> 世界负上
        // 相机前 -> 世界前
        const double X0 = rc.x;
        const double Y0 = rc.z;
        const double Z0 = -rc.y;

        const double c = std::cos(pitch);
        const double s = std::sin(pitch);

        // 绕 X 轴旋转
        Vec3 rw;
        rw.x = X0;
        rw.y = c * Y0 - s * Z0;
        rw.z = s * Y0 + c * Z0;

        return rw;
    }

    // =========================
    // 像素点 -> 地面世界坐标
    // =========================
    //
    // 输入像素点 (u, v)，计算它在地面 Z=0 上对应的世界点 (X, Y)
    //
    // 注意：
    // 1. 如果这条射线朝天或者平行地面，则 invalid
    // 2. fx, fy 用像素单位
    // 3. H 的单位决定输出世界坐标单位
    //
    inline GroundPoint imagePixelToGround(
        double u,
        double v,
        const CameraParam& cam)
    {
        // 1) 像素坐标 -> 相机归一化坐标
        Vec3 rc;
        rc.x = (u - cam.cx) / cam.fx;
        rc.y = (v - cam.cy) / cam.fy;
        rc.z = 1.0;

        // 2) 相机射线 -> 世界射线
        Vec3 rw = cameraRayToWorldRay(rc, cam.pitch);

        // 3) 相机中心在世界坐标中的位置
        // Cw = (0, 0, H)
        // 射线方程：P(t) = Cw + t * rw
        //
        // 与地面 Zw = 0 相交：
        // H + t * rw.z = 0  =>  t = -H / rw.z
        //
        GroundPoint gp{};
        gp.valid = false;

        // 射线没有指向地面，或者几乎平行地面
        if (std::abs(rw.z) < 1e-12)
            return gp;

        const double t = -cam.H / rw.z;

        // 只接受“向前”的交点
        if (t <= 0.0)
            return gp;

        gp.X = t * rw.x;
        gp.Y = t * rw.y;
        gp.valid = true;
        return gp;
    }

    // =========================
    // 世界坐标 -> 俯视图像素
    // =========================
    //
    // 把地面点 (X, Y) 映射到输出俯视图中的 (bx, by)
    //
    // 输出图约定：
    // - 左边是 minX，右边是 maxX
    // - 上边是 maxY（更远处）
    // - 下边是 minY（更近处）
    //
    inline bool groundToBirdPixel(
        double X, double Y,
        const IPMParam& ipmParam,
        double& bx, double& by)
    {
        if (X < ipmParam.minX || X > ipmParam.maxX ||
            Y < ipmParam.minY || Y > ipmParam.maxY)
        {
            return false;
        }

        const double xRatio =
            (X - ipmParam.minX) / (ipmParam.maxX - ipmParam.minX);

        const double yRatio =
            (Y - ipmParam.minY) / (ipmParam.maxY - ipmParam.minY);

        // X 从左到右
        bx = xRatio * (ipmParam.outWidth - 1);

        // 希望“远处在图像上方”
        by = (1.0 - yRatio) * (ipmParam.outHeight - 1);

        return true;
    }

    // =========================
    // 双线性采样（灰度图）
    // =========================
    inline uint8_t bilinearSampleGray(
        const uint8_t* src,
        int width,
        int height,
        int stride,
        double u,
        double v)
    {
        if (u < 0.0 || v < 0.0 || u > width - 1.0 || v > height - 1.0)
            return 0;

        const int x0 = static_cast<int>(std::floor(u));
        const int y0 = static_cast<int>(std::floor(v));
        const int x1 = std::min(x0 + 1, width - 1);
        const int y1 = std::min(y0 + 1, height - 1);

        const double dx = u - x0;
        const double dy = v - y0;

        const double p00 = src[y0 * stride + x0];
        const double p10 = src[y0 * stride + x1];
        const double p01 = src[y1 * stride + x0];
        const double p11 = src[y1 * stride + x1];

        const double v0 = p00 * (1.0 - dx) + p10 * dx;
        const double v1 = p01 * (1.0 - dx) + p11 * dx;
        const double val = v0 * (1.0 - dy) + v1 * dy;

        return clampToByte(val);
    }

    // =========================
    // 双线性采样（RGB 三通道）
    // 每像素 3 字节，RGBRGB...
    // =========================
    inline void bilinearSampleRGB(
        const uint8_t* src,
        int width,
        int height,
        int stride,
        double u,
        double v,
        uint8_t outRGB[3])
    {
        if (u < 0.0 || v < 0.0 || u > width - 1.0 || v > height - 1.0)
        {
            outRGB[0] = outRGB[1] = outRGB[2] = 0;
            return;
        }

        const int x0 = static_cast<int>(std::floor(u));
        const int y0 = static_cast<int>(std::floor(v));
        const int x1 = std::min(x0 + 1, width - 1);
        const int y1 = std::min(y0 + 1, height - 1);

        const double dx = u - x0;
        const double dy = v - y0;

        const uint8_t* p00 = src + y0 * stride + x0 * 3;
        const uint8_t* p10 = src + y0 * stride + x1 * 3;
        const uint8_t* p01 = src + y1 * stride + x0 * 3;
        const uint8_t* p11 = src + y1 * stride + x1 * 3;

        for (int c = 0; c < 3; ++c)
        {
            const double v0 = p00[c] * (1.0 - dx) + p10[c] * dx;
            const double v1 = p01[c] * (1.0 - dx) + p11[c] * dx;
            const double val = v0 * (1.0 - dy) + v1 * dy;
            outRGB[c] = clampToByte(val);
        }
    }

    // =========================
    // 俯视图像素 -> 世界坐标
    // =========================
    //
    // 这是做“逆映射”的关键：
    // 对输出俯视图的每个像素，先求它在世界地面的点，
    // 再反算它在原图中的位置，最后从原图采样。
    //
    inline void birdPixelToGround(
        double bx,
        double by,
        const IPMParam& ipmParam,
        double& X,
        double& Y)
    {
        const double xRatio = bx / (ipmParam.outWidth - 1);
        const double yRatio = 1.0 - by / (ipmParam.outHeight - 1);

        X = ipmParam.minX + xRatio * (ipmParam.maxX - ipmParam.minX);
        Y = ipmParam.minY + yRatio * (ipmParam.maxY - ipmParam.minY);
    }

    // =========================
    // 世界地面点 -> 原图像素
    // =========================
    //
    // 已知世界点 (X, Y, 0)，反投影到输入图像，便于做逆映射采样。
    //
    inline bool groundToImagePixel(
        double X,
        double Y,
        const CameraParam& cam,
        double& u,
        double& v)
    {
        // 世界点 Pw = (X, Y, 0)
        // 相机中心 Cw = (0, 0, H)
        // 世界方向向量 d_w = Pw - Cw = (X, Y, -H)
        const double dwx = X;
        const double dwy = Y;
        const double dwz = -cam.H;

        // 需要把世界方向转回相机方向
        // cameraRayToWorldRay 里用的是：Rw = Rx(pitch) * base
        // 因此这里做逆旋转：Rx(-pitch)
        const double c = std::cos(cam.pitch);
        const double s = std::sin(cam.pitch);

        // 先逆旋转到未俯仰状态
        const double X0 = dwx;
        const double Y0 = c * dwy + s * dwz;
        const double Z0 = -s * dwy + c * dwz;

        // 再映射回相机坐标
        // base: [X0, Y0, Z0] = [xc, zc, -yc]
        const double xc = X0;
        const double yc = -Z0;
        const double zc = Y0;

        // 在相机后方，无效
        if (zc <= 1e-12)
            return false;

        u = cam.fx * (xc / zc) + cam.cx;
        v = cam.fy * (yc / zc) + cam.cy;
        return true;
    }

    // =========================
    // 灰度图 IPM
    // =========================
    //
    // src: 输入灰度图
    // dst: 输出灰度图，需由外部分配 outHeight * dstStride 字节
    //
    inline void warpIPMGray(
        const uint8_t* src,
        int srcWidth,
        int srcHeight,
        int srcStride,
        uint8_t* dst,
        int dstStride,
        const CameraParam& cam,
        const IPMParam& ipmParam)
    {
        for (int by = 0; by < ipmParam.outHeight; ++by)
        {
            uint8_t* dstRow = dst + by * dstStride;

            for (int bx = 0; bx < ipmParam.outWidth; ++bx)
            {
                // 1) 输出俯视图像素 -> 世界地面点
                double X, Y;
                birdPixelToGround(static_cast<double>(bx),
                                  static_cast<double>(by),
                                  ipmParam, X, Y);

                // 2) 世界地面点 -> 原图像素
                double u, v;
                if (!groundToImagePixel(X, Y, cam, u, v))
                {
                    dstRow[bx] = 0;
                    continue;
                }

                // 3) 双线性采样
                dstRow[bx] = bilinearSampleGray(src, srcWidth, srcHeight, srcStride, u, v);
            }
        }
    }

    // =========================
    // RGB 图 IPM
    // =========================
    //
    // src: 输入 RGB 图，按 RGBRGB... 排列
    // dst: 输出 RGB 图，按 RGBRGB... 排列
    //
    inline void warpIPMRGB(
        const uint8_t* src,
        int srcWidth,
        int srcHeight,
        int srcStride,
        uint8_t* dst,
        int dstStride,
        const CameraParam& cam,
        const IPMParam& ipmParam)
    {
        for (int by = 0; by < ipmParam.outHeight; ++by)
        {
            uint8_t* dstRow = dst + by * dstStride;

            for (int bx = 0; bx < ipmParam.outWidth; ++bx)
            {
                double X, Y;
                birdPixelToGround(static_cast<double>(bx),
                                  static_cast<double>(by),
                                  ipmParam, X, Y);

                double u, v;
                if (!groundToImagePixel(X, Y, cam, u, v))
                {
                    uint8_t* p = dstRow + bx * 3;
                    p[0] = p[1] = p[2] = 0;
                    continue;
                }

                uint8_t rgb[3];
                bilinearSampleRGB(src, srcWidth, srcHeight, srcStride, u, v, rgb);

                uint8_t* p = dstRow + bx * 3;
                p[0] = rgb[0];
                p[1] = rgb[1];
                p[2] = rgb[2];
            }
        }
    }

}

Search

无 OpenCV 对图像逆透视

Key Insights

前言

逆透视变换

对应点对单应变换方法

简化相机模型 IPM方法