坐标系旋转推导和符号约定第一章 四套坐标系与角度符号总约定一、为什么旋转方向容易混淆旋转方向的混乱几乎全部来自一个事实同一套旋转矩阵在不同坐标系里的视觉方向是相反的。根本原因不在数学而在坐标轴的指向——尤其是 y 轴向上还是向下。很多人踩坑不是矩阵算错而是坐标系没对齐就套用了公式。本书的目的就是把坐标系先钉死再讲旋转让每个符号都有出处、可以复核。本章先给出四套坐标系的定义和角度正负的统一判定规则作为全书的地基。第 2 章在数学系里讲主动与被动的区别第 3 章转入图像坐标系第 5 章扩展到三维欧拉角第 6 章回到项目中的 roll 补偿链路。二、四套坐标系的定义一数学笛卡尔坐标系数学笛卡尔坐标系中x 轴向右y 轴向上构成右手系。绕原点逆时针转 θ 的主动旋转矩阵为R_act(θ) [ cosθ -sinθ ] [ sinθ cosθ ]这个矩阵不是凭记忆记下来的它来自单位圆上点的坐标变换。设单位圆上一点极角为 α坐标为 (cosα, sinα)。点绕原点逆时针转 θ 后极角变成 α 加 θ新坐标是 (cos(αθ), sin(αθ))。用三角和角公式展开cos(αθ) cosα·cosθ - sinα·sinθ sin(αθ) sinα·cosθ cosα·sinθ注意 cosα 正是原点的横坐标 xsinα 正是原点的纵坐标 y代入后整理成矩阵与向量相乘的形式[ x ] [ cosθ -sinθ ] [ x ] [ y ] [ sinθ cosθ ] [ y ]换一个角度验证矩阵的每一列单位向量 (1, 0) 绕原点逆时针转 θ 后应当落在 (cosθ, sinθ)这正好是矩阵第一列单位向量 (0, 1) 旋转 θ 后应当落在 (-sinθ, cosθ)这正好是矩阵第二列。矩阵的列就是基向量的像这个规律在第二章推导被动矩阵、第五章推导三维绕轴矩阵时会反复使用。约定 θ 大于零表示逆时针即从 x 轴往 y 轴方向转θ 小于零表示顺时针。这是教科书定义也是大多数数学库的默认约定。二OpenCV 图像坐标系图像坐标系的原点在图像左上角x 轴向右y 轴向下像素坐标 (u, v) 都在这一套坐标里。因为 y 轴翻转同一套矩阵的视觉方向会反过来数学里正 90 度是逆时针图像里正 90 度在画面上变成顺时针。图像坐标系下绕原点旋转的主动矩阵为R_img(θ) [ cosθ sinθ ] [ -sinθ cosθ ]其中 sinθ 的符号与数学矩阵相反完整推导放在第 3 章。三车体坐标系车体坐标系ego coordinate的轴约定在不同项目里并不一致这是最容易忽视的坑。原笔记开头的约定是 y 向右、z 向上绕 y 到 z 的轴对判定方向。而 stixel 项目实际使用的约定是 x 向前、y 向左、z 向上水平面内用 x 到 y 的轴对判定见 utils.cpp 中 Occupancy_Grid 的注释ego coordinate x: forward, y: left, meter。因此凡涉及车体系的代码必须先确认本项目的轴定义不能把两种约定混用。四相机坐标系相机坐标系以光心为原点x 轴向右y 轴向下z 轴沿光轴向前。它的 x、y 与图像系同向z 是深度方向。像素反投影函数 pixel_cam 的输出 (x, y, z) 就是这一套坐标其中 z 是沿光轴的深度。三、角度正负的统一判定规则无论在哪套坐标系都建议用右手定则结合轴对顺序判定正方向不要凭视觉猜测。数学系以 x 到 y 逆时针为正图像系因为 y 向下x 到 y 顺时针为正车体系在水平面内以 x 到 y 按右手定则绕 z 轴俯视逆时针为正相机系与图像系一致。这样每个角度的符号都能从坐标系推导出来而不是靠记忆。四、角度符号对照总表坐标系x 轴y 轴z 轴正角视觉方向负角视觉方向判定依据数学笛卡尔右上出纸面逆时针顺时针x→y 右手OpenCV 图像右下深度方向顺时针逆时针x→yy 翻转车体stixel 项目前左上俯视逆时针yaw俯视顺时针x→y 右手相机右下前光轴同图像系同图像系同图像系这张表是全书所有符号判定的总入口第 3、5、6 章会反复引用。五、使用建议写任何旋转代码之前先在第一行注释写清 x、y、z 的指向与角度正负约定这是成本最低的防错手段。不要跨坐标系直接套用矩阵数学矩阵用于图像坐标方向必然相反要么先把图像 y 取负转回数学系要么直接使用图像系矩阵。用数值自检代替肉眼判断把点 (1, 0) 旋转 θ看结果是否落在 (cosθ, sinθ) 上符号写反会在第一步就暴露具体验证清单见第 7 章。第二章 二维旋转主动与被动一、一句话区分主动变换Active是坐标系固定不动点或物体在空间里转动求转动后点在新位置下的原坐标系坐标。被动变换Passive是点或物体在空间里位置不变坐标系旋转求同一个空间点在新坐标系下的坐标。两条原则贯穿本章。第一条坐标系的旋转方向与点的等效旋转方向相反。第二条被动变换矩阵等于主动变换矩阵的转置也就是角度取负因为旋转矩阵是正交矩阵逆等于转置。本章全程使用第一章的数学笛卡尔坐标系x 向右、y 向上、右手系采用列向量约定矩阵左乘向量p’ 等于R_act(θ)乘以 p。二、主动变换点动坐标系不动绕原点逆时针转 θ点 p 等于 (x, y)变换后 p’ 等于R_act(θ)乘 p其中R_act(θ)主对角线为 cosθ副对角线为负 sinθ 与正 sinθR_act(θ) [ cosθ -sinθ ] [ sinθ cosθ ]验证场景取一个具体例子。点 p 等于 (1, 0)逆时针转 90 度cos90 度为 0sin90 度为 1代入矩阵得到 (0, 1)。在数学系里 y 轴向上点从 (1, 0) 转到 (0, 1)正好是正上方视觉上就是逆时针与直觉一致。这个例子正是第 7 章数值自检的基础旋转 (1, 0) 必须落在 (cosθ, sinθ)。再用矩阵列的角度复核一次作为第一章结论的二维延伸。把 p 写成基向量的线性组合 p 等于 x 乘 e_x 加 y 乘 e_y旋转是线性变换所以 p’ 等于 x 乘 e_x 的像加 y 乘 e_y 的像。e_x 等于 (1, 0) 的像是 (cosθ, sinθ)e_y 等于 (0, 1) 的像是 (-sinθ, cosθ)两个像恰好排成R_act(θ)的两列。于是矩阵乘法可以逐元素展开x cosθ·x - sinθ·y y sinθ·x cosθ·y这套逐元素写法在推导被动矩阵时会原样复现差别只在副对角线的符号。三、被动变换点不动坐标系转坐标系逆时针转 θ等价于从新坐标系的角度看点相对新坐标轴顺时针转了 θ。于是被动变换矩阵R_pass(θ)等于主动变换矩阵在角度上取负R_pass(θ) R_act(-θ) [ cosθ sinθ ] [ -sinθ cosθ ]原因是 cos(-θ) 等于 cosθsin(-θ) 等于负的 sinθ于是矩阵里副对角线符号反转。被动变换的验证场景点固定在 (1, 0)。坐标系逆时针转 90 度被动矩阵代入得 (0, -1)。物理含义是点还在原地但坐标轴逆时针转了 90 度从新坐标轴看这个点跑到了 y 轴负方向。这里的关键直觉是坐标系逆时针转 θ被动等价于点顺时针转 θ主动于是R_pass(θ)等于R_act(-θ)。四、极坐标推导为什么被动要减角度用极坐标可以把上面的符号关系看得更清楚。设空间点极径为 r初始极角为 α相对旧 x 轴那么 x₀ 等于 r 乘以 cosαy₀ 等于 r 乘以 sinα。先看主动变换的流程。点逆时针转 b新极角变成 α 加 b新坐标是 x’ 等于 r 乘 cos(αb)y’ 等于 r 乘 sin(αb)。把和角公式完整展开再把 x₀、y₀ 代回cos(αb) cosα·cosb - sinα·sinb sin(αb) sinα·cosb cosα·sinb x r·cos(αb) r·cosα·cosb - r·sinα·sinb x₀·cosb - y₀·sinb y r·sin(αb) r·sinα·cosb r·cosα·sinb x₀·sinb y₀·cosb写成矩阵形式正是R_act(b)乘以向量 (x₀, y₀)。再看被动变换的流程。点位置不变但坐标轴逆时针转 b点相对新 x 轴的极角变成 α 减 b。把差角公式完整展开cos(α-b) cosα·cosb sinα·sinb sin(α-b) sinα·cosb - cosα·sinb x r·cos(α-b) r·cosα·cosb r·sinα·sinb x₀·cosb y₀·sinb y r·sin(α-b) r·sinα·cosb - r·cosα·sinb x₀·sinb - y₀·cosb第二行把 x₀·sinb 减 y₀·cosb 写成 -sinb 乘 x₀ 加 cosb 乘 y₀得到矩阵形式正是R_pass(b)乘以向量 (x₀, y₀)副对角线为 sinb 与 -sinb。两组公式的差别问题落在极角是加 b 还是减 b 上其余符号全部来自三角函数的展开。直观例子r 等于 1α 等于 0点旧坐标 (1, 0)坐标系逆时针转 30 度新坐标是 cos(-30 度) 和 sin(-30 度)即约 (0.866, -0.5)点落在第四象限符合坐标轴旋转后的几何直觉。五、被动矩阵为什么等于主动矩阵的转置正交性问题贯穿本节。旋转矩阵 R 是正交矩阵满足 R 的转置乘以 R 等于单位阵于是 R 的逆等于 R 的转置。被动变换在几何上是主动变换的逆视角于是R_pass(θ)等于R_act(-θ)也等于R_act(θ)的转置。三个写法表达的是同一个矩阵主动逆时针 θ 用 cosθ 在主轴、负 sinθ 在副轴被动同样是坐标系逆时针 θ但副对角线符号反转。不过这一等价关系只对正交的纯旋转矩阵成立带缩放或错切的矩阵不满足转置关系。把正交性验证展开成具体乘法可以看到符号自洽的机制。用R_act(θ)乘它的转置R_act(θ)ᵀR·Rᵀ [ cosθ -sinθ ] [ cosθ sinθ ] [ sinθ cosθ ] [ -sinθ cosθ ] 第1行第1列: cos²θ sin²θ 1 第1行第2列: cosθ·sinθ - sinθ·cosθ 0 第2行第1列: sinθ·cosθ - cosθ·sinθ 0 第2行第2列: sin²θ cos²θ 1四个位置全部用平方和为 1、交叉项相消来归位结果正是单位阵。因为 R 乘 Rᵀ 等于单位阵R 的逆就等于 Rᵀ而R_act(-θ)与R_act(θ)ᵀ 逐元素相同三个写法因此等价。六、主动与被动对照表变换谁在动数学系矩阵点的新极角主动点动坐标系固定[[cosθ, -sinθ], [sinθ, cosθ]]α θ被动坐标系动点固定[[cosθ, sinθ], [-sinθ, cosθ]]α - θ几何关系一行R_pass(θ)等于R_act(-θ)等于R_act(θ)的转置方向相反。七、实际应用中最常见的混淆第一类混淆是把图像旋转当成坐标系旋转。OpenCV 的 warpAffine 和 getRotationMatrix2D 旋转的是像素点属于主动变换虽然它的矩阵形式看起来像被动矩阵但那是图像 y 轴向下的结果与主动被动无关这一点在第 3 章详细展开。第二类混淆出现在坐标变换上。相机到车体的变换R_vc描述的是同一个 3D 点在新坐标系车体下的坐标本质是被动视角即坐标变换而不是把物体转一个角度。凡是 cam2vehicle、ego2world 这类变换都要先问自己是点动还是坐标系动再决定用哪个矩阵。第三类混淆是方向说反。看到坐标系逆时针转条件反射就写主动矩阵忘记取负或转置。建议把第一章的对照总表贴在代码旁边写矩阵前先查符号。八、要点回顾主动变换中点动坐标系不动矩阵主对角线为 cosθ副对角线为负 sinθ 与正 sinθ。被动变换中坐标系动点不动矩阵等于主动矩阵取负角度也等于主动矩阵的转置。判定口诀是坐标系动等效于点反方向动遇到坐标变换先分清主动被动再写矩阵。下一步进入图像坐标系前建议先用本节要点做一次符号自检。第三章 OpenCV 图像坐标下的旋转一、图像坐标系的 y 轴向下意味着什么第一章已经指出图像坐标系 x 向右、y 向下。对旋转来说y 向下带来的直接后果是数学矩阵用在图像坐标里视觉方向会反转。同一个 θ数学系里是逆时针图像画面里变成顺时针。这一章把这个问题彻底拆开给出图像系自己的旋转矩阵并解释 OpenCV 的 getRotationMatrix2D 为什么正角度是逆时针。二、图像系旋转矩阵的推导y 取负法最稳妥的推导方法是把图像坐标映射到数学坐标旋转完再映射回来。设图像坐标为 (u, v)数学坐标为 (u, -v)。整个推导分三步每一步都写成显式公式。第一步图像坐标转数学坐标。数学横坐标与图像 u 相同数学纵坐标等于负的 v即 u_M 等于 uv_M 等于 -v。第二步在数学系里绕原点逆时针转 θ。数学点 (u_M, v_M) 经R_act(θ)作用u_M cosθ·u_M - sinθ·v_M cosθ·u - sinθ·(-v) cosθ·u sinθ·v v_M sinθ·u_M cosθ·v_M sinθ·u cosθ·(-v) sinθ·u - cosθ·v注意第二步里 v 的符号发生了翻转这正是图像矩阵副对角线符号反转的来源。第三步把结果映射回图像坐标。图像纵坐标等于负的数学纵坐标于是 v’ 等于 -v_M’ 等于 -sinθ 乘 u 加 cosθ 乘 v。两步结果合并u cosθ·u sinθ·v v -sinθ·u cosθ·v展开后得到图像系的主动旋转矩阵R_img(θ) [ cosθ sinθ ] [ -sinθ cosθ ]与数学矩阵对比副对角线的 sinθ 符号完全相反。这就是很多人在 OpenCV 里写手写矩阵时方向反了的根源直接把课本矩阵抄进来sinθ 符号没翻。三、数值验证点 (0, 1) 转 30 度用数值验证上面的矩阵。取图像点 (0, 1)它在画面上位于原点正下方y 向下。代入 R_img(30 度)得到 (0.5, 0.866)。这个点相对原点x 增大、y 仍为正但略减画面位置在右上方偏左视觉上是从正下方逆时针转向上方。结论图像系矩阵正角度确实对应画面逆时针。对照数学系同样输入 (0, 1)数学点 (0, 1) 在 y 轴正方向上方R_act(30 度) 得到 (-0.5, 0.866)视觉也是逆时针。两套矩阵各自在自己的坐标系里语义一致差异只在坐标定义不在旋转本身。四、getRotationMatrix2D 的角度定义OpenCV 的 cv::getRotationMatrix2D 返回 2 乘 3 仿射矩阵配合 warpAffine 使用。函数原型为 getRotationMatrix2D(center, angle, scale)其中 angle 单位是度正值表示画面逆时针旋转负值表示顺时针。注意它内部的三角函数把角度转成弧度调用时直接传度数即可。函数返回的矩阵为M [ α β (1-α)·c_x - β·c_y ] [ -β α β·c_x (1-α)·c_y ] α scale · cosθ β scale · sinθ其中 (c_x, c_y) 是旋转中心。这个矩阵与第三节手写的 R_img 完全一致区别只是多了一个把中心平移到原点的平移项以及缩放因子 scale。五、手写矩阵与 getRotationMatrix2D 的对照表想要的画面效果手写图像系矩阵 angle弧度getRotationMatrix2D angle度逆时针 90 度-90 · π/18090顺时针 90 度90 · π/180-90注意表格里手写矩阵一栏的符号它沿用第三节 R_img 的推导正角度在图像系里对应画面逆时针但那是用 R_img 的矩阵形式如果你把课本数学矩阵 R_act 直接抄进图像系正角度就变成画面顺时针。因此写代码前先确认用的是哪套矩阵形式。六、可运行的 Python 验证脚本下面这个脚本验证两件事手写图像系矩阵与 getRotationMatrix2D 结果一致点绕中心旋转 90 度后位置正确。importcv2importnumpyasnpdefrotate_point():# 原始点 (200,100)旋转中心 (200,200)pointnp.array([200,100],dtypenp.float64)centernp.array([200,200],dtypenp.float64)# 图像系手写矩阵正角度 画面逆时针angle_deg-90# 画面逆时针 90 度thetaangle_deg*np.pi/180.0R_imgnp.array([[np.cos(theta),np.sin(theta)],[-np.sin(theta),np.cos(theta)]])p_relpoint-center p_rotR_img p_relcenter# OpenCV 参考实现同样传 90逆时针Mcv2.getRotationMatrix2D(center(200,200),angle90,scale1.0)p_cvM np.array([200,100,1.0])print(hand-written:,np.round(p_rot,3))print(getRotationMatrix2D:,np.round(p_cv,3))assertnp.allclose(p_rot,p_cv[:2]),两套实现不一致if__name____main__:rotate_point()运行后两行输出一致即验证通过。如果改成手写数学矩阵 R_act 并传 -90 度输出会和 getRotationMatrix2D(90) 不一致这正是本章强调的符号陷阱。七、一个统一逻辑的替代方案如果不想被图像 y 向下反复干扰可以把所有点先做一次 y 取负全部按数学系处理算完再取负还原。代价是每个点都要多一步变换适合点数量少、需要与数学推导对照的场景。像素级操作如 warpAffine仍然是图像系矩阵更快。八、要点回顾图像系旋转矩阵副对角线的 sinθ 与数学矩阵反号来源是 y 轴向下。getRotationMatrix2D 正角度对应画面逆时针内部自动转弧度。写代码前先确认矩阵形式是 R_img 还是 R_act再用数值自检验证。第四章 绕任意中心的旋转与代码实现一、为什么不能只讲绕原点前面两章的矩阵都是绕原点旋转。实际图像处理里旋转中心几乎从不是原点例如 OpenCV 里绕图像中心、或 stixel 项目里绕矫正后主点做 roll 补偿。绕任意点 (c_x, c_y) 旋转需要在旋转矩阵外面套一层平移把中心先搬到原点转完再搬回去。本章完成完整的推导、数值示例和可运行的 C 实现。二、三步变换的齐次推导绕任意中心旋转加等比例缩放等价于三个连续变换的复合。第一步平移 T1把旋转中心移动到坐标原点第二步绕原点旋转并缩放 R_s第三步平移 T2把中心搬回原位。写成 3 乘 3 齐次矩阵T1 [ 1 0 -c_x ] [ 0 1 -c_y ] [ 0 0 1 ] R_s [ α β 0 ] [ -β α 0 ] [ 0 0 1 ] T2 [ 1 0 c_x ] [ 0 1 c_y ] [ 0 0 1 ]其中 α 等于 scale 乘 cosθβ 等于 scale 乘 sinθ。注意 R_s 使用的是图像系矩阵形式右上角是正 β这与第三章的结论一致。三、合并成 2 乘 3 仿射矩阵把三个矩阵按 T2 乘 R_s 乘 T1 的顺序相乘。先算中间结果 R_s·T1注意它仍然是一个 3 乘 3 齐次矩阵第三列是旋转与第一次平移的复合R_s·T1 [ α β -α·c_x - β·c_y ] [ -β α β·c_x - α·c_y ] [ 0 0 1 ]推导第三列的过程如下T1 的第三列是 (-c_x, -c_y, 1)R_s 乘它得到第一行 α 乘 (-c_x) 加 β 乘 (-c_y)第二行 -β 乘 (-c_x) 加 α 乘 (-c_y)第三行保持 1。再把 T2 左乘上去。T2 的作用是给前两行分别加 c_x 与 c_y第三行不变M T2·(R_s·T1) [ α β -α·c_x - β·c_y c_x ] [ -β α β·c_x - α·c_y c_y ] [ 0 0 1 ]把第三列整理成 (1-α) 乘 c_x 减 β 乘 c_y、β 乘 c_x 加 (1-α) 乘 c_y去掉恒等第三行合并的目标是得到 2 乘 3 仿射矩阵M [ α β (1-α)·c_x - β·c_y ] [ -β α β·c_x (1-α)·c_y ]验证中心点不动。把 (c_x, c_y) 代入变换式x’ 等于 α 乘 c_x 加 β 乘 c_y 加 (1-α) 乘 c_x 减 β 乘 c_y其中 β 乘 c_y 与减 β 乘 c_y 相消剩下 α 乘 c_x 加 (1-α) 乘 c_x 等于 c_xy’ 同理β 乘 c_x 与减 β 乘 c_x 相消剩下 α 乘 c_y 加 (1-α) 乘 c_y 等于 c_y。中心点经过变换后位置不变这是矩阵正确性的第一个自检点。四、数值示例数值验证场景取中心 (100, 100)角度 30 度scale 1.0。θ 等于 30 度转弧度cos 约 0.866025sin 约 0.5于是 α 约 0.866025β 约 0.5。平移项t_x 等于 (1 减 0.866025) 乘 100 减 0.5 乘 100约 -36.6025t_y 等于 0.5 乘 100 加 (1 减 0.866025) 乘 100约 63.3975。矩阵为M [ 0.866025 0.5 -36.6025 ] [ -0.5 0.866025 63.3975 ]代入中心点 (100, 100) 验证x’ 等于 86.6025 加 50 减 36.6025等于 100y’ 等于 -50 加 86.6025 加 63.3975等于 100。中心不动通过。再取一个非中心点比如 (0, 0)x’ 等于 -36.6025y’ 等于 63.3975点绕中心逆时针 30 度并保持距离方向正确。五、C 手写实现下面的实现修复了原笔记里shtudouble的笔误应为 at 方法并给出完整可编译的主函数输出与 getRotationMatrix2D 逐元素一致#includeopencv2/opencv.hpp#includeiostreamcv::MatmyGetRotationMatrix2D(cv::Point2f center,doubleangle_deg,doublescale){doublethetaangle_deg*CV_PI/180.0;doublealphascale*std::cos(theta);doublebetascale*std::sin(theta);doublecxcenter.x;doublecycenter.y;cv::MatM(2,3,CV_64F);M.atdouble(0,0)alpha;M.atdouble(0,1)beta;M.atdouble(0,2)(1.0-alpha)*cx-beta*cy;M.atdouble(1,0)-beta;M.atdouble(1,1)alpha;M.atdouble(1,2)beta*cx(1.0-alpha)*cy;returnM;}intmain(){cv::Point2fcenter(100.0f,100.0f);cv::Mat minemyGetRotationMatrix2D(center,30.0,1.0);cv::Mat refcv::getRotationMatrix2D(center,30.0,1.0);std::coutmine:\nmine\nref:\nref\n;doubleerrcv::norm(mine-ref);std::coutdiff errstd::endl;// 期望接近 0return0;}编译运行后 diff 应接近 0。如果手写矩阵用了数学系形式第一行第二列会是负 βdiff 必然非零符号问题立刻暴露。六、容易踩坑的五个点角度单位问题的关键在于度与弧度的换算。角度单位必须是度函数内部才转弧度直接传弧度会放大或缩小旋转量。β 的符号必须与图像系一致抄数学矩阵就反了。scale 是各向同性的不支持单独缩放 x 或 y。返回的是 2 乘 3 矩阵warpAffine 内部自动补第三行。矩阵只保证旋转中心不变图像旋转后超出原图边界的部分会被裁剪需要完整保留时额外修正平移项。七、要点回顾绕任意中心的旋转等于平移、旋转、平移的复合合并公式与 getRotationMatrix2D 逐项一致。中心点不变是首要自检diff 与 OpenCV 实现逐元素比对是第二道自检。下一步进入三维建议先按本章的 diff 自检确认二维实现无误再进入绕轴矩阵。第五章 三维旋转与欧拉角约定一、从二维到三维维度问题的起点是自由度。二维旋转只有一个自由度三维旋转需要三个角度。本章先给出三个绕轴旋转矩阵再讲欧拉角 yaw、pitch、roll 在车体系里的定义与旋转顺序最后把主动被动关系延伸到三维并落到项目标定数据上。这一章是第六章 roll 补偿链路的地基。二、三个绕轴旋转矩阵在右手笛卡尔坐标系x 右、y 上、z 出纸面里绕 x、y、z 轴逆时针转 θ 的主动矩阵分别为Rx(θ) [ 1 0 0 ] [ 0 cosθ -sinθ ] [ 0 sinθ cosθ ] Ry(θ) [ cosθ 0 sinθ ] [ 0 1 0 ] [ -sinθ 0 cosθ ] Rz(θ) [ cosθ -sinθ 0 ] [ sinθ cosθ 0 ] [ 0 0 1 ]这三个矩阵的构造逻辑是同一个绕哪根轴旋转那根轴的分量就不变剩下两根轴在它们张成的平面里做二维旋转。以 Rx 为例绕 x 轴转 θ 时 x 坐标保持y、z 分量在 yz 平面内绕原点旋转yz 平面内的二维矩阵恰好嵌在右下角。矩阵的每一列仍然等于基向量的像逐一验证三组Rx 作用 (1,0,0) → (1, 0, 0) x 轴不动 Rx 作用 (0,1,0) → (0, cosθ, sinθ) y 轴在 yz 平面内转 θ Rx 作用 (0,0,1) → (0, -sinθ, cosθ) z 轴同平面内转 θ绕 y 轴需要小心符号因为 x、z 轴绕 y 正轴逆时针转时x 轴是转向 z 的负方向。验证方法同样用基向量像e_x 等于 (1,0,0) 的像是 (cosθ, 0, -sinθ)e_z 等于 (0,0,1) 的像是 (sinθ, 0, cosθ)两列正好是 Ry 的第一列与第三列。绕 z 轴则与二维完全一致左上角的 2 乘 2 块就是R_act(θ)。校验目标很明确三个矩阵的行列式都为 1属于纯旋转。主动与被动的关系在三维同样成立。坐标变换矩阵是被动视角等于对应主动矩阵的转置。连续旋转的复合顺序不同结果就不同这正是欧拉角最需要注意的地方。三、车体系的轴定义与欧拉角三个欧拉角的定义场景如下。yaw 是绕 z 轴的旋转描述车头方向在水平面内的偏转。pitch 是绕 y 轴的旋转描述车体俯仰。roll 是绕 x 轴的旋转描述车体侧倾。三个角度的正方向都按右手定则绕轴正向逆时针为正。对摩托车场景左侧有脚撑导致车体向左倾斜roll 取负值表示左倾。这一点直接对应代码注释里的约定roll 小于零为左倾补偿时方向要按此符号处理。四、旋转顺序先 roll 再 pitch 再 yaw旋转顺序问题是欧拉角的第一道坎。欧拉角必须声明旋转顺序否则同一组角度会得到不同的旋转结果。项目里构造车体到世界姿态矩阵 makeOdom_Ego 使用 Rz(yaw) 乘 Ry(pitch) 乘 Rx(roll) 的顺序即先绕 x 轴转 roll再绕 y 轴转 pitch最后绕 z 轴转 yaw。写成矩阵R Rz(yaw) · Ry(pitch) · Rx(roll)先展开中间两项 Ry(pitch) 乘 Rx(roll)记 pitch 为 p、roll 为 r。逐列相乘第一列来自 e_x 的像第二列、第三列按矩阵乘法规则展开Ry(p)·Rx(r) [ cosp sinp·sinr sinp·cosr ] [ 0 cosr -sinr ] [ -sinp cosp·sinr cosp·cosr ]验证第二列Rx(r) 把 (0,1,0) 送到 (0, cosr, sinr)Ry(p) 再作用第一行得 sinp 乘 sinr第二行得 cosr第三行得 cosp 乘 sinr与上式一致。第三列同理。再左乘 Rz(yaw)记 yaw 为 y得到完整的复合矩阵R [ cosy·cosp cosy·sinp·sinr - siny·cosr cosy·sinp·cosr siny·sinr ] [ siny·cosp siny·sinp·sinr cosy·cosr siny·sinp·cosr - cosy·sinr ] [ -sinp cosp·sinr cosp·cosr ]这个展开式可以直接用于审查 makeOdom_Ego把代码里的 yaw、pitch、roll 代入上式逐元素比对符号与位置错一处都会立刻暴露。它也从代数上说明复合顺序不可交换Rz 乘 Ry 与 Ry 乘 Rz 的结果不同先声明顺序再谈角度。这个顺序属于外旋绕固定轴依次旋转从右往左读就是先 roll 后 pitch 后 yaw。审查代码时先确认旋转顺序与代码一致再谈角度符号。五、相机坐标系与车体系的差异相机坐标系 x 右、y 下、z 沿光轴向前与车体系 y 左、z 上不同两根轴方向相反。因此相机到车体的变换不能只写一个角度必须用完整的姿态旋转矩阵R_vc编码 yaw、pitch、roll 三个分量。标定文件里的外参例如前视相机 yaw 约 -90.06 度、pitch 约 0.4 度、roll 约 -101.7 度描述的就是相机相对车体的完整姿态R_vc由这些欧拉角构造属于被动变换把同一个 3D 点从相机坐标转到车体坐标。六、符号验证的两个自检拿到任何欧拉角构造的旋转矩阵先做两个数值自检。把单位向量沿 x 轴正方向 (1, 0, 0) 代入 R应当得到该轴旋转后的方向。再验证 R 的转置乘以 R 等于单位阵行列式为 1。roll 为负时沿 x 轴正向看y 轴方向应当向下摆动这与脚撑左倾的物理含义一致。具体验证代码放在第七章。七、要点回顾本章的任务是把三维旋转拆成三个角度的复合。三维旋转由 yaw、pitch、roll 三个角按 Rz 乘 Ry 乘 Rx 的顺序复合。车体系 x 前、y 左、z 上roll 为负表示左倾。相机系与车体系轴方向不同必须用完整R_vc编码姿态。下一步进入 roll 补偿链路前建议先用本节要点核对 makeOdom_Ego 的矩阵构造顺序。第六章 项目实战roll 补偿链路符号自洽一、场景与问题摩托车前后各装一套双目相机左侧有脚撑驻车时车体会向左倾斜按第五章约定 roll 小于零。车身倾斜会让双目图像里的地平线和障碍物随之倾斜同时相机到车体、车体到世界的坐标变换也携带这个 roll。项目在四个位置分别处理了 roll本章逐一核对符号证明整条链路端到端自洽。二、链路四步与符号第一步是图像级 roll 补偿。主流程对矫正后的双目图做 warpAffine绕矫正后主点旋转前视传入负 roll后视由于参考相机朝后、成像右轴近似车体左侧旋转方向取反。第二步是 stixel 检测内部的反向补偿get_visual 在把像素画回输出图时做一次像素反旋转角度取负的补偿角。第三步是 3D 重建cluster_world_coordinate 用 pixel_cam 把像素反投影到相机系再用标定姿态 R_vc 转到车体系。第四步是 ego2world用定位给的 roll、pitch、yaw 按 Rz 乘 Ry 乘 Rx 顺序把车体系转到世界系。三、为什么四处 roll 能两两抵消把四步写成符号链抵消关系可以逐级验证。设车身实际 roll 为 φ按第五章约定左倾时 φ 小于零。图像 warp 把倾斜图像转到水平视角等价于在像素层面施加 R_img(-φ) 的主动旋转stixel 反旋转把 stixel 像素转回真实倾斜相机系施加 R_img(φ)是 warp 的逆操作。两步复合用第三章的 R_img 形式展开R_img(-φ)·R_img(φ) [ cosφ -sinφ ] [ cosφ sinφ ] [ sinφ cosφ ] [ -sinφ cosφ ] 第1行第1列: cos²φ sin²φ 1 第1行第2列: cosφ·sinφ - sinφ·cosφ 0 第2行第1列: sinφ·cosφ - cosφ·sinφ 0 第2行第2列: sin²φ cos²φ 1结果为单位阵像素域的两步严格互逆图像先转平再转回原姿态像素位置不变。这是链路的前两级抵消。后两级发生在坐标域。pixel_cam 输出相机系坐标 X_camcam2vehicle 用标定姿态矩阵 R_vc 转到车体系ego2world 用定位姿态矩阵 R_wv 转到世界系。完整链条写成X_ego R_vc · X_cam X_world R_wv · X_ego Rz(yaw)·Ry(pitch)·Rx(roll定位) · R_vc · X_cam标定姿态 R_vc 本身包含 roll 标定 分量定位姿态 R_wv 包含 roll 定位 分量。在符号约定一致的前提下若两个 roll 同源且数值相等Rx(roll定位) 与 Rx(roll标定) 互为逆坐标域的 roll 部分相消世界系水平面不残留 roll。只要四处的符号约定一致世界坐标就不会残留 roll 误差。四、逐点核对表位置代码位置roll 处理作用图像 warpmain.cpp 前视与后视分支前视负 roll后视取反图像转到水平视角像素反旋转StixelDetect.cpp get_visual角度取负补偿角像素转回真实相机系cam2vehicleutils.cpp cluster_world_coordinateR_vc 标定姿态相机系到车体系ego2worldutils.cpp makeOdom_EgoRz·Ry·Rx 定位姿态车体系到世界系四处的符号由第一章对照表和第五章旋转顺序统一导出链路设计正确。五、最大风险roll 同源链路成立的前提是图像补偿与 ego2world 使用同一个 roll 来源。当前图像补偿用 cameraData.camera_loc.rollego2world 用融合定位的 location.roll两者若不同源例如驻车静态倾角被定位滤波成零抵消链立即失效误差约为 sin(roll) 乘距离三度倾角每米就有约五厘米的横向偏差。审查与测试时应把两个 roll 打日志对比确认同源或显式归一。六、验证建议实车在前后视野各放已知坐标的地面标记物对比检测输出的世界坐标。若系统性偏差随距离线性增大优先检查 roll 同源若偏差为常量优先检查平移 t_vc 与高度配置。世界系水平不残留 roll是本链路正确性的最终判据。七、要点回顾roll 补偿链路由图像 warp、像素反旋转、cam2vehicle、ego2world 四步组成符号按第一章与第五章约定逐点核对后两两抵消。最需要盯住的是 roll 同源问题以及用地面标记物做端到端验证。第七章 验证方法、常见坑与对照表汇总一、旋转矩阵的验证方法写对旋转矩阵最快的方式不是背公式而是让矩阵自己证明自己。下面四条验证规则前两条在代码里最常用后两条用于矩阵构造类函数。第一中心点不变。对 getRotationMatrix2D 这类带旋转中心的矩阵把中心代入变换式输出必须还是中心代数上就是第四章末尾的相消验证。第二基准点轨迹正确。把点 (1, 0) 旋转 θ结果必须是 (cosθ, sinθ)写成公式就是 R 的第一列必须等于 (cosθ, sinθ) 的转置符号写反在这里立刻暴露。第三正交性。旋转矩阵 R 满足 R 的转置乘 R 等于单位阵行列式为 1浮点误差应小于 1e-6。第四正反变换互逆。主动矩阵与它的转置互为逆连续施加两次被动变换等于一次主动变换。正交性检验可以写成一个可复用的数值断言用于所有矩阵构造函数Rᵀ·R I (逐元素误差 1e-6) det(R) 1 (符号为负说明混入了镜像) R·(1,0,0)ᵀ 应等于矩阵第一列前两条规则用向量像验证后两条用矩阵性质验证四者组合覆盖了旋转矩阵的全部关键性质。二、常见坑清单先列出最容易踩的五类坑。第一坐标系没声明就套矩阵数学矩阵用在图像坐标方向必反。第二把图像旋转当成坐标系旋转或反过来混淆主动与被动。第三欧拉角不声明旋转顺序同一组 yaw、pitch、roll 按不同顺序复合结果不同。第四角度单位混用函数收度你传弧度旋转量错得离谱。第五roll 同源问题图像补偿与坐标变换用不同来源的 roll抵消链失效。三、最终对照表项目数学系图像系车体系stixel 项目x / y / z右 / 上 / 出纸面右 / 下 / 深度前 / 左 / 上主动矩阵绕 zθ 大于 0[[cosθ,-sinθ],[sinθ,cosθ]][[cosθ,sinθ],[-sinθ,cosθ]]同数学系三维 Rz被动矩阵主动转置同左坐标变换视角getRotationMatrix2D 正角度—画面逆时针—roll 为负含义——车体左倾四、自测检查清单逐项确认可以独立复现。写出数学系主动矩阵并旋转 (1, 0) 验证落点。写出图像系矩阵并验证与 getRotationMatrix2D 一致。用手写 C 实现与 OpenCV 输出做 diff期望为零。用欧拉角 Rz·Ry·Rx 构造矩阵验证正交性与行列式。对比图像补偿 roll 与定位 roll 的日志确认同源。五、全书收束从第一章的坐标系总表到第二章主动与被动第三章图像系符号翻转第四章任意中心旋转第五章三维欧拉角再到第六章 roll 补偿链路核心只有一句话任何旋转都要先声明坐标系与旋转顺序再写矩阵最后用数值自检验证。对照表与自测清单可以直接贴在代码旁作为检查工具。注部分内容由豆包工作 AI 生成