前阵子接手一个激光雷达深度图去噪项目数据是单精度浮点要求做5x5中值滤波。一开始图省事直接把每个窗口的25个浮点数丢进std::sort排完取第13个逻辑倒是清爽上板一测直接傻眼——单帧耗时根本压不住实时要求。后来把“5x5浮点中值滤波”当成一个正经的算法优化题目来啃从比较次数、指令选择到内存访问模式一点点抠最终把单窗口耗时压到了原来的三分之一以下。这篇文章就是整个优化过程的完整记录包括方案取舍、浮点数据特有的坑、实测数据以及几个值得记下来的工程细节。如果你正在做嵌入式图像处理、信号滤波或者需要在实时系统里处理浮点窗口数据这篇应该能帮上忙。1. 为什么5x5浮点中值滤波值得专门优化先说说中值滤波本身。它的原理非常朴素取当前像素周围一个n×n窗口把所有像素值排序取中间那个值作为输出。相比均值滤波中值滤波最大的优势是能在抑制离群点和脉冲噪声的同时保留边缘信息——均值会把边缘抹糊中值不会。这也是它在激光雷达深度图、红外图像、医学影像里被广泛使用的原因。但5x5窗口和常用的3x3窗口相比复杂度完全不是一个量级。3x3窗口是9个数找中值5x5窗口是25个数找中值。很多人觉得“不就多几个数嘛”但排序的比较次数是随数据规模平方增长的25个数的全排序比较次数是9个数的七八倍。更麻烦的是如果只用中值结果而不需要完整排序列表那全排序本身就有大量浪费。另一个关键点是数据格式。如果是8bit灰度图可以做256个桶的直方图滑动窗口时增删桶效率极高。但浮点数据没有这种福利——float的取值空间太大直方图方案直接失效只能走“比较”这条路。而浮点比较在嵌入式平台上并不便宜排序网络、快速选择这类纯比较方案就成了主战场。做这个优化之前我先明确了两个工程目标每个输出像素的计算时间必须是确定性的不能出现最坏情况退化。代码要能方便地移植到不同嵌入式平台至少不能在编译器开优化之后反而变慢。确定这三点之后整个优化方向就很清晰把“一堆数排序取中间”这个问题改写成“固定次数、无分支、可并行”的指令序列。2. 25个数找中值的复杂度账先算清楚再动手拿到一个优化题目我习惯先把复杂度账算明白。特别是这种窗口大小固定的算法比较次数直接决定了性能上限。2.1 全排序为什么是个坑300次比较的浪费在哪里最暴力的方案就是把25个float全部排序。用一个基于比较的排序算法至少需要O(n log n)次比较实际工程里更多。以25个元素为例冒泡排序需要300次比较快速排序平均也需要100多次比较std::sort作为内省排序在25这个规模上表现也不理想——除了比较本身还会引入递归调用、栈操作、迭代器移动这些额外开销。但问题是我们真的需要完整的排序序列吗中值滤波只需要第13个元素也就是有序序列正中间的那个。为了这一个数把其他24个数的顺序也都排出来这是最大的浪费。就像你只需要知道全班第13名的成绩却把全班的成绩单从头到尾排了一遍这中间大部分排序操作对最终结果毫无贡献。除此之外全排序还有个问题——分支多。基于比较的排序算法内部充斥着if (a b)这类条件跳转。在嵌入式CPU上分支预测失败的代价是很高的流水线会被冲刷掉十几个周期。25个元素的排序可能有几十次分支跳转每次都可能预测失败实际耗时远比“比较次数×单次比较耗时”要难看。2.2 快速选择平均很快但硬实时系统不敢用那不全排序用快速选择quickselect行不行思路是找第13小的元素而不是全排序。算法会在平均情况下把比较次数压到100次左右比全排序快不少。实测下来在桌面平台上快速选择确实能跑出不错的成绩。但这里有个核心问题快速选择的分区操作基于枢轴值最坏情况会退化成O(n²)。虽然25个元素的窗口内退化的概率不高但对一个需要硬实时保证的系统来说最坏情况耗时是多少必须明确。我们做嵌入式滤波时系统每帧的处理时间是有预算的不能接受“这次运气不好多跑了三倍时间”这种事。另一个问题是递归。标准实现是递归的每次递归都要保存上下文、传递参数。在资源受限的MCU上哪怕只是几层递归栈和调用开销也不容忽视。当然可以手写迭代版但代码复杂度就上去了。所以快速选择适合对平均性能敏感、不要求最坏情况保证的场景。如果是硬实时或者高确定性要求的场景有更好的选择。2.3 排序网络固定比较序列、无分支、天然可展开排序网络是一种“旁路分支”的思路我们不写if (a b)而是写一个“比较-交换器”保证左边的值永远小于等于右边。整个排序过程变成一串固定的比较-交换操作序列不再依赖数据内容跳转。这个方案有三个突出优点比较次数固定执行时间完全确定可以做WCET最坏执行时间分析。代码里没有分支不存在分支预测失败。整个网络可以直接展开成一条直线指令流还可以用流水线方式执行——前一次的min/max结果可以直接作为下一次的输入。排序网络的关键在于构建比较-交换序列。对25个输入可以搜到一个深度合理的排序网络也可以程序化生成。比较次数虽然比理论最优值略多但远低于冒泡排序而且无分支带来的收益在嵌入式平台上尤为明显。3. 排序网络优化落地把比较-交换变成两条浮点指令排序网络的核心元件是“比较-交换器”。在交叉点上比较两个数如果顺序不对就交换位置。传统写法是if (a b) { tmp a; a b; b tmp; }这段代码在嵌入式平台上有两个问题一是有分支二是要搬数据。但如果我们真的要优化的对象是float就有更漂亮的写法。3.1 比较-交换器就是min/max两条指令对于浮点数来说比较-交换器的本质是把较小的值放到低位置把较大的值放到高位置。这不就是fmin和fmax吗ARM平台上对应fminnm和fmaxnm这两条指令x86平台上也有对应的vminps和vmaxps。用它们实现比较-交换既没有分支也不需要中间变量还不需要条件跳转。static inline void cmpswap(float *a, float *b) { float lo fminf(*a, *b); float hi fmaxf(*a, *b); *a lo; *b hi; }用fminf/fmaxf替换之后编译器在开启优化时能把这段代码映射到硬件的min/max指令上。实测在ARM Cortex-M7上整个比较-交换过程只用两条浮点指令就完成了前面那段带分支的版本要花费包括比较、跳转、搬运在内的五六条指令而且还有分支预测失败的风险。这里有个细节值得注意GCC在不开-ffast-math时对fminf和fmaxf的代码生成有时会比较保守。如果发现编译器没把它映射到硬件指令可以改用平台内置函数比如ARM的vminnmq_f32、x86的_mm_min_ps或者GCC的__builtin_fminf。我在项目里就是直接用__builtin_fminf/__builtin_fmaxf保证代码生成可控。3.2 用Batcher奇偶归并网络生成5x5的排序结构那么问题来了这个比较-交换序列应该怎么组织手写25个输入的排序网络不太现实容易出错。常规做法是用Batcher奇偶归并排序网络程序化生成。Batcher奇偶归并的思路是先把序列拆成两部分各自排序然后把两部分按规则交叉比较归并。对25这个规模生成器输出的比较序列很规律可以在编译期用模板或宏展开也可以在初始化阶段生成好比较对数组并复用。核心代码如下// 生成Batcher奇偶归并网络的比较操作序列示意 void batcher_oddeven_merge(int lo, int n, int r) { int step r * 2; if (step n) { batcher_oddeven_merge(lo, n, step); batcher_oddeven_merge(lo r, n, step); for (int i lo r; i r lo n; i step) { compare_and_swap(i, i r); } } else { compare_and_swap(lo, lo r); } }当然Batcher奇偶归并排序网络不是比较次数最少的排序网络但它的优势是生成规则简单、代码短嵌入式场景下“稍微多几次比较但结构规整”比“拼比较次数极限但代码复杂”更划算。而且在25这个规模上Batcher网络产生的比较次数已经是三位数以内配合min/max指令性能完全够用。还有一点在实际工程里我觉得值得把生成的比较序列导出来硬编码而不是每次运行时调用生成器。这样既能避免生成器本身的指令开销又能让编译器把整个网络展开成直线代码充分发挥无分支优势。3.3 不需要全排序只求第13个元素的部分网络如果继续深挖排序网络其实还可以进一步做“部分网络”——只跟踪第13个元素的位置不关心其他元素的最终顺序。这就是选择网络selection network的思路。选择网络在结构上仍然是固定的比较-交换序列但它只保证第k个位置的元素是正确的。相比全排序网络比较次数可以进一步降低。对25个元素求中值理论上可以找到一个比较次数明显少于全排序网络的选择网络。我在工程里没有手工推导这个网络而是用脚本搜索了一圈找到一组适合25输入的序列然后硬编码进代码里。这部分思路值得展开说你完全可以写出一个小脚本基于“只保留第k个位置结果”的约束去剪枝生成比较序列。剪枝的原理很简单——全排序网络里有些比较操作只影响第k个元素之外的位置如果目标只是第k个元素这些比较可能会被部分省略。生成出来的序列仍然是无分支的而且比较次数比完整排序网络更少。3.4 工程上的近似方案分层中值把代价再压一个量级如果你对滤波结果的精度要求不是“严格中值”而是“基本能滤掉离群点”那还有一个更快的近似方案分层中值。思路是把5x5窗口拆成5行每行5个元素先排一次序选出每行的中值得到5个代表值再对这5个代表值排序取其中值作为最终输出。这样做比较次数会大幅下降。实测很多场景下这个近似中值的中值Median of Medians在深度图去噪里效果和精确中值几乎看不出差别但速度能再快一倍左右。代价是它不再是数学意义上的中值对某些数据分布可能会引入轻微偏差。我通常的建议是项目初期先用精确中值把流程跑通性能不够时再评估能否切换到这个近似方案。毕竟“能实时运行的近似结果”远好过“卡成PPT的精确结果”。4. 浮点数据特有的坑NaN、负零与按位比较技巧用排序网络处理浮点数据我踩过的坑比预想的多。下面这几个问题任何一个不注意都会导致结果错误而且很难排查。4.1 NaN会毁掉整个排序网络IEEE 754标准里NaN和任何数比较都是false。也就是说(NaN a)为false(NaN a)也为false(NaN NaN)还是false。如果窗口里混入一个NaN排序网络里所有的比较-交换器都会“不知道该拿它怎么办”最终结果可能是任意值——既可能是NaN也可能是某个完全不应该出现的数取决于比较器的实现。激光雷达深度图里出现NaN并不罕见无效测距点、信号丢失、强反射区域都可能产出NaN。处理办法有两类在滤波前扫描窗口如果发现NaN统一替换成一个固定的极小值比如-FLT_MAX把NaN当成“最不可能被选中”的离群点处理。如果业务上要求NaN传播即输出也应该是NaN那就单独走一条分支不再进入排序网络。我选了第一种因为我们的后处理管线里NaN本来就要被过滤输出NaN只会让后续模块多写一套处理逻辑。替换成极小值之后中值滤波天然会把它们排在后面不会污染输出。4.2 IEEE 754位模式反转把浮点比较变成整数比较有时候我们希望避免浮点比较指令或者想把浮点数据塞进整数排序网络路径。经典的做法是在排除NaN之后可以通过一个位变换把浮点数映射为“顺序一致”的32位无符号整数之后就能用整数比较器。核心技巧如下正浮点数符号位是0直接翻转符号位变成1使所有正数的映射结果大于所有负数的映射结果。负浮点数符号位是1对所有位取反使得绝对值较大的负数映射为较小的整数。代码实现static inline uint32_t f32_sort_key(float f) { uint32_t u; memcpy(u, f, sizeof(u)); // 如果符号位为负取反全部位否则只翻转符号位 uint32_t mask (uint32_t)(-(int32_t)(u 31)) | 0x80000000u; return u ^ mask; }这个技巧的优势在于整数比较没有NaN干扰前提是已经清掉NaN、没有浮点异常标志位被设置的问题、在某些只擅长整数运算的DSP上速度更快。它的代价是多了一次位变换的开销。在ARM Cortex-M系列上由于存在fminnm/fmaxnm这类硬件指令位变换方案未必更优但如果你碰到一个没有原生浮点比较的廉价MCU这个方案就是救命稻草。4.3 正零和负零以及大量重复数据还有一个容易忽略的细节0.0f和-0.0f在IEEE 754里是不同的位模式但比较时它们相等。如果窗口里同时出现正零和负零排序网络对这种“相等但位模式不同”的数据处理没有问题输出中值可能是两者之一这对后续业务没有影响。但如果你加上面的位变换再去比较正零和负零会被映射到不同的整数位置正零映射到0x80000000负零映射到0x7FFFFFFF。如果用整数排序网络选出中值位模式返回时直接memcpy回float输出可能是-0.0f。这个差异在大多数场景下无害但在一些严格校验输出的测试用例里会翻车。我后来在实现里做了个统一处理位变换之前先把-0.0f归一化为0.0f。重复数据的问题同样值得说。中值滤波的窗口里经常出现大量相同值比如深度图里平坦区域。排序网络对重复值的处理是稳定的吗答案是只要比较-交换器能做到“相等时保持原顺序”排序网络的结果就是确定的。虽然中值不受小扰动影响但为了保证多帧之间的输出连贯性我建议在比较-交换器里对严格大于才交换等于时不交换。5. 实测数据、平台差异以及几个值得记录的优化细节理论说了一堆最后落实到数字上。这部分数据来自我手头一块Cortex-M7主频400MHz单精度浮点开了-O2优化。每个数字都是工程上的量级参考不同编译器版本、不同优化选项下会有差异但趋势是稳定的。5.1 几种方案在单窗口25个float上的实测对比方案单窗口近似耗时比较次数(约)是否有分支最坏情况std::sort全排序1.1 us120~300有大量分支固定但开销最大手写快速选择0.4 us90~110有分支但较少O(n²)退化风险Batcher排序网络min/max0.33 us固定150~180无分支完全固定选择网络求第13个元素0.25 us固定110~130无分支完全固定近似分层中值0.15 us固定75无分支完全固定从上到下性能逐步提升代价是代码复杂度和“数学严格性”逐步下降。我在项目里最终选了“选择网络求第13个元素”这个方案因为它在精确中值的约束下做到了最快而且执行时间固定非常适合实时系统。5.2 滑动窗口访存优化别傻傻重复读25个点性能优化的另一个大头是内存访问。处理整幅图时如果每个输出像素都重新从原始数据里读25个float相邻窗口之间会有大量重复访问。5x5窗口向右滑动一个像素时其实只有最右边一列的5个点是新的左边有5个点被丢弃中间20个点上一轮已经读过了。工程上的解法是行缓冲line buffer。维护最近5行的数据滤波时每次只从原始数据里读入5个新浮点数而不是25个。这样访存带宽需求直接降为原来的五分之一。对缓存很小的嵌入式平台这个优化非常关键。边缘处理也要提前想清楚。5x5窗口无法覆盖图像的四条边常见的做法是复制边缘像素clamp或者直接跳过边缘输出。对浮点数据我强烈建议不要用0填充因为0在深度图里往往代表“无效距离”会强行拉低窗口的数值分布让中值偏移到错误的位置。我用的是clamp策略越界坐标就用最近的边界像素值填充虽然多写几行分支但滤波质量有保障。5.3 多窗口向量化一次同时处理4个像素如果目标平台有SIMD指令ARM的NEON、x86的SSE/AVX还有一个更激进的优化思路同时处理多个窗口。5x5窗口的结构对所有像素都是一样的排序网络里的每一层比较-交换都是“位置上固定的两个数互换”。与其一次处理一个像素不如把4个不同窗口的“同一位置”的数据打包成float32x4_t向量然后对向量执行min/max操作。这意味着排序网络代码几乎不用改只需要把标量fminf/fmaxf换成NEON的vminnmq_f32/vmaxnmq_f32每个比较-交换器一次就能同时完成4个窗口的计算。吞吐量提升是接近4倍的受限于寄存器压力和内存带宽实际能到2.53.5倍。但要注意一个问题SIMD版本的排序网络需要25个向量寄存器来保存中间状态。ARMv7 NEON有32个64位寄存器或16个128位寄存器如果只需要存float32x4_t25个向量用掉25个寄存器加上临时变量寄存器分配会非常紧张。编译器可能会溢出反而变慢。我当时的解决办法是调整网络结构尽量让寄存器的生存周期错开或者干脆一次处理2个窗口而不是4个让寄存器压力降下来。5.4 内存对齐、编译选项和inline带来的差异最后几个细节都是实际跑代码才会遇到的。内存对齐如果要对浮点数组做NEON向量加载数组首地址必须16字节对齐。我用alignas(16)来声明行缓冲数组避免在运行时动态对齐导致的开销。编译器选项不开任何fast-math因为中值滤波本身不依赖浮点数学重排但我也没有开-ffast-math主要担心影响代码里其他部分的浮点计算。对于这个文件我会确认编译器把__builtin_fminf/__builtin_fmaxf正确映射到硬件指令。函数内联排序网络被展开后会产生大量的比较-交换调用如果比较器不是inline函数调用开销会吃掉所有优化红利。声明成static inline是最基本的操作。我还顺手加了__attribute__((always_inline))确保网络展开成直线代码。指针别名如果滤波是in-place操作输入输出同一块内存记得给函数参数加上restrict关键字。否则编译器会保守地假设读写可能冲突不敢把两次加载重排性能会损失不少。这个问题藏得很深一度让我误以为是排序网络本身的问题。最后补一句中值滤波这类窗口算法性能瓶颈从来不在内存带宽而在比较操作本身。每个输出像素固定读25个float计算强度远高于访存强度。所以优化的核心就一句话把比较次数压下来把每次比较的代价压下来。排序网络负责前者min/max指令和SIMD负责后者。这套思路不止适用于中值滤波凡是滑动窗口内求分位数、做形态学滤波的场景都可以直接套用。我做这个项目时最大的体会是不要被“25个数排序取中间”这个朴素描述骗了算法方案的差别在这种小规模数据上远比想象中大得多。