OpenCVなしの画像逆透視
はじめに
自動運転技術の発展に伴い、より多くの人々が最新のテクノロジーに触れ、コンピュータの世界がどのように自動運転を実現しているのか、また自動運転システムに付随する特定の機能がどのように実装されているのか、ますます好奇心を抱くようになりました。今回は、システムの中で 「360° バックカメラ映像」 の背後にあるアルゴリズムのロジックを説明し、実践します。
逆透視変換
画像取得時には、車両全体が複数のカメラを呼び出し、合成して一枚の「360°パノラマ写真」を作成します。
そして、俯瞰視点の 「360° バックカメラ映像」 を形成するには、数学的な演算、すなわち逆透視変換(略称IPM)を経る必要があります。
この分野には、IPM変換の方法がいくつかあり、例えば「対応点対ホモグラフィ変換法」、「簡略化カメラモデル逆透視変換」などがありますが、いずれも行列の変換則を利用しています。
対応点対ホモグラフィ変換法
この変換方法は比較的シンプルなので、詳細な説明は省略します。
少なくとも4つの対応点ペアを入力し、3点以上が同一直線上にあってはなりません。カメラのパラメータや平面位置に関する情報は必要ありません。点ペアを利用して透視変換行列を求めます。この行列は3次の正方行列であり、線形方程式を構築して解くことができます。4点より多い場合は、$ransac$ 法を用いて解くことができ、点の選び方は通常、手動で選択し、一般的には消失点を選びます。

この変換はコード上で比較的簡単に実装でき、比較的容易に IPM 変換を実現できますので、ここでは詳しく説明せず、コード例も提供しません。
簡略化カメラモデルIPM法
これは今回重点的に解析する変換方法です。このアルゴリズムの本質は、カメラの撮像過程における様々な座標間の変換関係を利用し、それを抽象化・簡略化して、最終的に世界座標を得ることです。
そして、世界座標と画像座標の対応関係を構築し、その関係を用いて数学的変換を行います。
複雑で長大な計算式とは異なり、ここでは引き続き座標演算を用います。この IPM 計算方法では、まずカメラの実際のパラメータを測定する必要があります。
ここでは、仰角 は 、中心高さ は 、視点から視平面までの距離 は として、世界座標 を求めます。
カメラ画像座標を とし、世界座標と画像座標の関係から行列方程式を構築します。
画像座標を式$(1)$に代入して、世界座標の行列を求めると、次式のようになります。
、$B = -d$、$C = d\sin\theta - \frac{H}{d}$、$D = \cos\theta$、$E = d\sin\theta$ とおき、幾何学的関係から が成り立ちます。これにより、$\boldsymbol{P_W}$ の最も簡略化された形式は次式で表されます。
最後に、画像を処理します。処理する画像は二次元平面図であるため、画像の深さは常に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];
}
}
}
}