OpenCV を使わない画像の逆透視変換

#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チャンネル）
    // 1ピクセルあたり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];
            }
        }
    }

}