根据之前的文篇，可以得到相关的公式
像素坐标系相机坐标系自车ego坐标系世界坐标系
双目立体视觉视差推导与 StereoRectify 算法原理报告
坐标系旋转推导和符号约定
针孔相机视差相似三角形推导
stixel-world柱状物检测公式推导
Slanted-Stixels柱状物斜坡检测公式推导
双目视差计算深度推导
Slanted Stixels 与坡路拟合：原理与工程实现详解
Stixel World 原理深度解析报告
来自 https://github.com/gishi523/stixel-world/blob/master/stixel_world.cpp#L20-L52
// Transformation between pixel coordinate and world coordinate /** * 像素 (行号 v + 视差 d) ↔ 世界 (高度 Y + 前方距离 Z) 的双向换算。 * * 坐标系约定（工程内统一）： * Y : 从相机光心向下的垂直距离，向下为正 [m]，路面在 Y = +camera.height * Z : 沿水平路面向前的距离 [m] * θ = camera.tilt : 相机下俯角（光轴相对水平面向下倾斜），下俯为正 [rad] * v : 图像行号，向下增大；v0 主点行；视差 d [px] * * 核心几何（绕 x 轴转 θ，世界 → 相机系）： * z_cam = Y·sinθ + Z·cosθ // 沿光轴的深度 * y_cam = Y·cosθ − Z·sinθ // 图像纵向分量（垂直于光轴） * 成像：v − v0 = fv·y_cam/z_cam ； d = baseline·fu / z_cam * 联立反解 → toY/toZ；顺序代入 → toV/toD */ struct CoordinateTransform { CoordinateTransform(const CameraParameters& camera) : camera(camera) { // 预计算 sin/cos，避免每个像素重复算三角函数 sinTilt = (sinf(camera.tilt)); cosTilt = (cosf(camera.tilt)); // B = baseline·fu/fv：打包常数，使 B/d = z_cam/fv。 // 视差→深度用 fu（水平焦距），像素→角度用 fv（垂直焦距）， // 二者不等时原版公式（假设 fu==fv）会失真，B 把比例合进一个数。 B = camera.baseline * camera.fu / camera.fv; } // (d, v) → Y：该像素的世界高度（向下为正）。 // 路面像素 d = roadDisp[v] 算出来应 ≈ camera.height。 // 推导：Y = z_cam·(sinθ + (v−v0)·cosθ/fv) = (B/d)·((v−v0)·cosθ + fv·sinθ) inline float toY(float d, int v) const { return (B / d) * ((v - camera.v0) * cosTilt + camera.fv * sinTilt); } // (d, v) → Z：该像素的前方距离。 // 推导：Z = z_cam·(cosθ − (v−v0)·sinθ/fv) = (B/d)·(fv·cosθ − (v−v0)·sinθ) inline float toZ(float d, int v) const { return (B / d) * (camera.fv * cosTilt - (v - camera.v0) * sinTilt); } // (Y, Z) → v：世界点投回图像行号。 // 例：FreeSpace 里 YT = YB − objectHeight 后再 toV， // 就是"路面以上 objectHeight 米的障碍物顶在哪一行"。 inline float toV(float Y, float Z) const { return camera.fv * (Y * cosTilt - Z * sinTilt) / (Y * sinTilt + Z * cosTilt) + camera.v0; } // (Y, Z) → d：该世界点应有的视差。 // 例：HeightSegmentation 的 deltaD = dB − toD(YB, ZB + deltaZ)， // 比较"同高、远 deltaZ 米"处的视差差，决定像素属于物体还是地面。 inline float toD(float Y, float Z) const { return camera.baseline * camera.fu / (Y * sinTilt + Z * cosTilt); } CameraParameters camera; // 一份参数拷贝（仅几个 float） float sinTilt, cosTilt, B; // 预计算的三角函数与打包常数 };Y c a m = Y c o s θ − Z ∙ s i n θ Y_{cam}=Ycosθ−Z∙sinθYcam=Ycosθ−Z∙sinθ①Z c a m = Y s i n θ + Z ∙ c o s θ Z_{cam}=Ysinθ+Z∙cosθZcam=Ysinθ+Z∙cosθ②
联立①和②，可得③④
Y = Z c a m s i n θ + Y c a m c o s θ Y=Z_{cam}sinθ+Y_{cam}cosθY=Zcamsinθ+Ycamcosθ③Z = Z c a m c o s θ − Y c a m s i n θ Z=Z_{cam}cosθ−Y_{cam}sinθZ=Zcamcosθ−Ycamsinθ④
空间点投影到图像行：v = v 0 + f v Y c a m Z c a m v=v_0+\frac{f_vY_{cam}}{Z_{cam}}v=v0+ZcamfvYcam⑤ ，相似三角形
视差d = b f u Z c a m d=\frac{bf_u}{Z_{cam}}d=Zcambfu⑥ ，联立⑤⑥得Z c a m = b f u d Z_{cam}=\frac{bf_u}{d}Zcam=dbfu⑨
Y c a m = b f u f v v − v 0 d Y_{cam}=\frac{bf_u}{f_v}\frac{v-v_0}{d}Ycam=fvbfudv−v0⑩
将⑨⑩代入③④，得Y = b f u f v 1 d [ ( v − v 0 ) c o s θ + f v s i n θ ] Y=\frac{bf_u}{f_v}\frac{1}{d}[(v−v_0)cosθ+f_vsinθ]Y=fvbfud1[(v−v0)cosθ+fvsinθ]
Z = b f u f v 1 d [ f v c o s θ − ( v − v 0 ) s i n θ ] Z=\frac{bf_u}{f_v}\frac{1}{d}[f_vcosθ−(v−v_0)sinθ]Z=fvbfud1[fvcosθ−(v−v0)sinθ]
①②代入⑤⑥求出v和d
v = f v Y c o s θ − Z ∙ s i n θ Y s i n θ + Z ∙ c o s θ + v 0 v=f_v\frac{Ycosθ−Z∙sinθ}{Ysinθ+Z∙cosθ}+v_0v=fvYsinθ+Z∙cosθYcosθ−Z∙sinθ+v0
d = b f u Y s i n θ + Z ∙ c o s θ d=\frac{bf_u}{Ysinθ+Z∙cosθ}d=Ysinθ+Z∙cosθbfu
需要注意到：B = camera.baseline * camera.fu / camera.fv;，也就是B = b f u f v B = \frac{bf_u}{f_v}B=fvbfu
其中，Y = b f u f v 1 d [ ( v − v 0 ) c o s θ + f v s i n θ ] Y=\frac{bf_u}{f_v}\frac{1}{d}[(v−v_0)cosθ+f_vsinθ]Y=fvbfud1[(v−v0)cosθ+fvsinθ]是toY函数，return (B / d) * ((v - camera.v0) * cosTilt + camera.fv * sinTilt);
Z = b f u f v 1 d [ f v c o s θ − ( v − v 0 ) s i n θ ] Z=\frac{bf_u}{f_v}\frac{1}{d}[f_vcosθ−(v−v_0)sinθ]Z=fvbfud1[fvcosθ−(v−v0)sinθ]是toZ函数，return (B / d) * (camera.fv * cosTilt - (v - camera.v0) * sinTilt);
v = f v Y c o s θ − Z ∙ s i n θ Y s i n θ + Z ∙ c o s θ + v 0 v=f_v\frac{Ycosθ−Z∙sinθ}{Ysinθ+Z∙cosθ}+v_0v=fvYsinθ+Z∙cosθYcosθ−Z∙sinθ+v0是toV函数，return camera.fv * (Y * cosTilt - Z * sinTilt) / (Y * sinTilt + Z * cosTilt) + camera.v0;
d = b f u Y s i n θ + Z ∙ c o s θ d=\frac{bf_u}{Ysinθ+Z∙cosθ}d=Ysinθ+Z∙cosθbfu是toD函数，return camera.baseline * camera.fu / (Y * sinTilt + Z * cosTilt);
