这段代码实现了一个 One Euro Filter（一欧元滤波器）。
1. 原理简述
One Euro Filter 是一种主要用于人机交互（如 VR 手柄追踪、光标移动、手势识别）的算法。它解决了一个核心矛盾：抖动（Jitter）与延迟（Lag/Latency）之间的权衡。
-
问题：
-
如果你为了让信号平滑（去抖动）而使用强滤波，会导致画面跟不上手的移动（高延迟）。
-
如果你为了响应快（低延迟）而减少滤波，画面会不停晃动（高抖动）。
-
-
解决方案：
-
动态调整截止频率。
-
当速度慢时（手停在半空）：认为用户想精确定位，增加滤波强度（降低截止频率），消除抖动。
-
当速度快时（手快速挥动）：认为用户关注的是速度和方向，减少滤波强度（提高截止频率），消除延迟。
import numpy as np class OneEuroFilter: def __init__( self, min_cutoff=1.0, # 最小截止频率 (Hz)。值越小，静止时越平滑（抖动越少），但迟滞感越强。 beta=0.0, # 速度系数。值越大，快速移动时响应越快（延迟越低），但可能引入高频噪声。 d_cutoff=1.0, # 导数（速度）的截止频率 (Hz)。用于平滑速度的计算，通常设为 1.0Hz。 ): """初始化 One Euro Filter，支持 N 维 Numpy 数组（注释中提到的 14 维只是示例）。""" self.min_cutoff = float(min_cutoff) self.beta = float(beta) self.d_cutoff = float(d_cutoff) # 用于存储数据的形状，确保后续输入数据维度一致 self.data_shape = None # 初始化上一帧的状态：时间、信号值、导数（速度） self.t_prev = None # 上一次的时间戳 self.x_prev = None # 上一次过滤后的信号值 self.dx_prev = None # 上一次过滤后的导数（速度） # 指定平滑函数，通常是指数平滑算法 self.smoothing_fn = exponential_smoothing def next(self, t, x, dx0=None): """ 计算下一帧的过滤信号。 参数: t: 当前时间戳 (秒) x: 当前原始信号值 (Numpy array) dx0: (可选) 初始速度 """ # --- 1. 初始化阶段 --- # 如果是第一次调用（没有上一帧数据），直接返回当前原始值作为初始状态 if self.t_prev is None: self.data_shape = x.shape # 记录数据维度 self.t_prev = float(t) # 记录当前时间 self.x_prev = np.array(x, dtype=float) # 记录当前值 # 初始化速度（导数），如果没有提供 dx0，则默认为 0 if dx0 is None: self.dx_prev = np.zeros_like(x) else: self.dx_prev = np.array(dx0, dtype=float) return x # --- 2. 数据检查 --- # 确保传入的数据维度没有发生改变 if x.shape != self.data_shape: raise ValueError("Unexpected data shape") # --- 3. 计算时间间隔 --- # t_e (Time Elapsed): 当前帧与上一帧的时间差 t_e = t - self.t_prev # --- 4. 计算并平滑速度 (Derivative) --- # 这一步是为了获取平滑的“速度”，用来动态调整后续的截止频率。 # 计算速度滤波的 alpha 值 (平滑因子) # d_cutoff 是固定的，通常设为 1Hz，用于避免速度计算本身抖动过大 a_d = smoothing_factor(t_e, self.d_cutoff) # 计算原始速度：(当前位置 - 上次位置) / 时间间隔 dx = (x - self.x_prev) / t_e # 对速度进行指数平滑 # dx_hat 即为“估计速度” dx_hat = self.smoothing_fn(a_d, dx, self.dx_prev) # --- 5. 计算并平滑主信号 (Signal) --- # 这是 One Euro Filter 的核心：动态计算 cutoff (截止频率) # 公式：cutoff = min_cutoff + beta * |速度| # 速度越快，cutoff 越高，滤波越弱，延迟越低。 # 速度越慢，cutoff 越接近 min_cutoff，滤波越强，越稳。 cutoff = self.min_cutoff + self.beta * np.abs(dx_hat) # 根据动态计算出的 cutoff 计算主信号的 alpha 值 a = smoothing_factor(t_e, cutoff) # 对主信号进行指数平滑 x_hat = self.smoothing_fn(a, x, self.x_prev) # --- 6. 更新状态 --- # 将当前计算出的平滑值和时间保存，供下一帧使用 self.x_prev = x_hat self.dx_prev = dx_hat self.t_prev = t return x_hat补充缺失的辅助函数
代码片段中调用了 smoothing_factor 和 exponential_smoothing，在标准的 One Euro Filter 实现中，它们的逻辑如下：
import numpy as np import math def smoothing_factor(t_e, cutoff): """ 计算指数平滑的 alpha 值 (0 <= alpha <= 1)。 alpha 决定了新数据在结果中的占比。 原理公式: tau = 1 / (2 * pi * cutoff) alpha = 1 / (1 + tau / t_e) """ r = 2 * math.pi * cutoff * t_e return r / (r + 1) def exponential_smoothing(a, x, x_prev): """ 执行指数移动平均 (Exponential Moving Average)。 公式: x_hat = alpha * x_raw + (1 - alpha) * x_prev a: alpha (平滑因子) x: 当前原始输入 x_prev: 上一次的平滑输出 """ return a * x + (1 - a) * x_prev总结：参数如何调节？
在使用这个类时，调节 min_cutoff 和 beta 是关键：
-
先调 min_cutoff (保持 beta = 0)：
-
将物体保持静止，观察输出数据。
-
如果数据还在抖动，减小 min_cutoff。
-
直到静止时的抖动在你可接受的范围内（此时延迟会比较大，但在下一步解决）。
-
-
再调 beta：
-
快速移动物体。
-
如果感觉跟随有明显的滞后（延迟），增加 beta。
-
增加 beta 会让高速运动时的截止频率变大，从而减少延迟。但如果 beta 太大，高速运动结束时可能会有“过冲”或微小抖动。
-
-
这个算法非常适合处理鼠标光标、VR 头显、Kinect 骨骼数据等实时连续信号。
-
这段代码定义了一个 LPRotationFilter (低通旋转滤波器) 类。
它的主要作用是对旋转数据（四元数或旋转矩阵）进行平滑处理，消除传感器（如 VR 手柄、机械臂末端、摄像头）带来的高频抖动，使旋转动作看起来更平稳。
import numpy as np
from scipy.spatial.transform import Rotation as R
# 假设外部定义的旋转平滑函数（通常是球面线性插值 SLERP 或归一化线性插值 NLERP）
# def rotational_exponential_smoothing(alpha, current, previous): ...
class LPRotationFilter:
"""
来源: https://github.com/Dingry/bunny_teleop_server/...
这是一个针对旋转数据的低通滤波器 (Low Pass Filter)。
"""
def __init__(self, alpha):
"""
初始化滤波器。
参数:
alpha: 平滑系数 (0 < alpha <= 1)。
alpha 越小，平滑程度越高，抗抖动越强，但延迟越大。
alpha 越大，响应越快，延迟越小，但抖动可能保留。
"""
self.alpha = alpha # 保存平滑系数
self.is_init = False # 标记是否已经初始化（是否接收过第一帧数据）
self.y = None # 保存上一帧的滤波结果 (y 代表 output)
def next(self, x: np.ndarray):
"""
输入当前的四元数，返回平滑后的四元数。
参数:
x: 当前帧的旋转四元数，形状必须是 (4,)，即 [x, y, z, w] 或 [w, x, y, z]
"""
# 强制检查输入形状是否为 4 维向量（四元数标准形状）
assert x.shape == (4,)
# --- 初始化逻辑 ---
# 如果是第一次运行，没有“上一帧”，直接把当前帧作为初始状态
if not self.is_init:
self.y = x # 记录当前值
self.is_init = True # 标记已初始化
return self.y.copy() # 返回副本，防止外部修改影响内部状态
# --- 滤波逻辑 ---
# 调用外部的旋转指数平滑函数。
# 原理类似：y_new = alpha * x_current + (1 - alpha) * y_prev
# 但因为是四元数，不能直接加减，需要用球面插值 (SLERP/NLERP) 处理。
self.y = rotational_exponential_smoothing(self.alpha, x, self.y)
# 返回平滑后的四元数副本
return self.y.copy()
def next_mat(self, x: np.ndarray):
"""
输入旋转矩阵，返回平滑后的旋转矩阵。
这是一个 wrapper (包装器)，方便直接处理矩阵格式的数据。
参数:
x: 3x3 旋转矩阵 或 4x4 变换矩阵
"""
# 检查输入形状是否合法
assert x.shape == (3, 3) or x.shape == (4, 4)
# 如果输入是 4x4 矩阵（通常是齐次变换矩阵），只取左上角 3x3 的旋转部分
if x.shape == (4, 4):
x = x[:3, :3]
# 1. 转换：矩阵 (Matrix) -> 四元数 (Quaternion)
# 使用 scipy 的 Rotation 库进行转换，因为四元数更适合做插值平滑
x = R.from_matrix(x).as_quat()
# 2. 滤波：调用上面的 next() 方法对四元数进行平滑
next_x_quat = self.next(x)
# 3. 还原：四元数 (Quaternion) -> 矩阵 (Matrix)
# 将平滑后的四元数转回旋转矩阵返回
return R.from_quat(next_x_quat).as_matrix()
def reset(self):
"""重置滤波器状态，清除历史数据"""
self.y = None
self.is_init = False
这段代码定义了一个 LPRotationFilter (低通旋转滤波器) 类。
它的主要作用是对旋转数据（四元数或旋转矩阵）进行平滑处理，消除传感器（如 VR 手柄、机械臂末端、摄像头）带来的高频抖动，使旋转动作看起来更平稳。
1. 代码逐行注释与解释
为了代码能运行，我们假设代码中隐含引用了 scipy.spatial.transform.Rotation (简称 R) 以及一个外部函数 rotational_exponential_smoothing。
import numpy as np
from scipy.spatial.transform import Rotation as R
# 假设外部定义的旋转平滑函数（通常是球面线性插值 SLERP 或归一化线性插值 NLERP）
# def rotational_exponential_smoothing(alpha, current, previous): ...
class LPRotationFilter:
"""
来源: https://github.com/Dingry/bunny_teleop_server/...
这是一个针对旋转数据的低通滤波器 (Low Pass Filter)。
"""
def __init__(self, alpha):
"""
初始化滤波器。
参数:
alpha: 平滑系数 (0 < alpha <= 1)。
alpha 越小，平滑程度越高，抗抖动越强，但延迟越大。
alpha 越大，响应越快，延迟越小，但抖动可能保留。
"""
self.alpha = alpha # 保存平滑系数
self.is_init = False # 标记是否已经初始化（是否接收过第一帧数据）
self.y = None # 保存上一帧的滤波结果 (y 代表 output)
def next(self, x: np.ndarray):
"""
输入当前的四元数，返回平滑后的四元数。
参数:
x: 当前帧的旋转四元数，形状必须是 (4,)，即 [x, y, z, w] 或 [w, x, y, z]
"""
# 强制检查输入形状是否为 4 维向量（四元数标准形状）
assert x.shape == (4,)
# --- 初始化逻辑 ---
# 如果是第一次运行，没有“上一帧”，直接把当前帧作为初始状态
if not self.is_init:
self.y = x # 记录当前值
self.is_init = True # 标记已初始化
return self.y.copy() # 返回副本，防止外部修改影响内部状态
# --- 滤波逻辑 ---
# 调用外部的旋转指数平滑函数。
# 原理类似：y_new = alpha * x_current + (1 - alpha) * y_prev
# 但因为是四元数，不能直接加减，需要用球面插值 (SLERP/NLERP) 处理。
self.y = rotational_exponential_smoothing(self.alpha, x, self.y)
# 返回平滑后的四元数副本
return self.y.copy()
def next_mat(self, x: np.ndarray):
"""
输入旋转矩阵，返回平滑后的旋转矩阵。
这是一个 wrapper (包装器)，方便直接处理矩阵格式的数据。
参数:
x: 3x3 旋转矩阵 或 4x4 变换矩阵
"""
# 检查输入形状是否合法
assert x.shape == (3, 3) or x.shape == (4, 4)
# 如果输入是 4x4 矩阵（通常是齐次变换矩阵），只取左上角 3x3 的旋转部分
if x.shape == (4, 4):
x = x[:3, :3]
# 1. 转换：矩阵 (Matrix) -> 四元数 (Quaternion)
# 使用 scipy 的 Rotation 库进行转换，因为四元数更适合做插值平滑
x = R.from_matrix(x).as_quat()
# 2. 滤波：调用上面的 next() 方法对四元数进行平滑
next_x_quat = self.next(x)
# 3. 还原：四元数 (Quaternion) -> 矩阵 (Matrix)
# 将平滑后的四元数转回旋转矩阵返回
return R.from_quat(next_x_quat).as_matrix()
def reset(self):
"""重置滤波器状态，清除历史数据"""
self.y = None
self.is_init = False
2. 原理解析
这个类的核心原理结合了指数移动平均 (EMA) 和 四元数几何 (Quaternion Geometry)。
A. 为什么要用这个类？(One Euro Filter vs LP Filter)
这只是一个简单的 低通滤波器 (Low Pass Filter, LPF)，它的 alpha 是固定的。
-
对比上一条的 One Euro Filter：One Euro Filter 会根据速度动态调整 alpha。而这个 LPRotationFilter 的 alpha 是恒定的。
-
适用场景：适合噪声比较稳定，或者对延迟要求不是极其苛刻的场景。
因此，代码中调用的 rotational_exponential_smoothing 内部通常实现的是 NLERP (Normalized Linear Interpolation) 或 SLERP (Spherical Linear Interpolation)。
C. next_mat 的工作流
很多机器人学库（如 numpy, eigen）习惯用 3x3 矩阵或者是 4x4 变换矩阵来表示位姿。
但是矩阵很难直接进行平滑插值（直接对矩阵元素做平均会破坏正交性，导致矩阵不再是旋转矩阵）。
所以标准流程是：
-
Matrix -> Quaternion: 转成四元数。
-
Filter: 在四元数空间进行平滑（数学上更健壮）。
-
Quaternion -> Matrix: 转回矩阵供其他程序使用。
完整代码如下：
import math
import numpy as np
from numba import jit
from scipy.spatial.transform import Rotation as R
from scipy.spatial.transform import Slerp
@jit
def smoothing_factor(t_e, cutoff):
r = 2 * math.pi * cutoff * t_e
return r / (r + 1)
@jit
def exponential_smoothing(a, x, x_prev):
return a * x + (1 - a) * x_prev
def rotational_exponential_smoothing(a, x, x_prev):
s = Slerp([0, 1], R.from_quat([x_prev, x]))
x_hat = s(a)
return x_hat.as_quat()
class OneEuroFilter:
def __init__(
self,
min_cutoff=1.0,
beta=0.0,
d_cutoff=1.0,
):
"""Initialize the one euro filter for a 14-dimensional numpy array."""
self.min_cutoff = float(min_cutoff)
self.beta = float(beta)
self.d_cutoff = float(d_cutoff)
self.data_shape = None
self.t_prev = None
self.x_prev = None
self.dx_prev = None
self.smoothing_fn = exponential_smoothing
def next(self, t, x, dx0=None):
"""Compute the filtered signal for a 14-dimensional numpy array."""
if self.t_prev is None:
self.data_shape = x.shape
self.t_prev = float(t)
self.x_prev = np.array(x, dtype=float)
if dx0 is None:
self.dx_prev = np.zeros_like(x)
else:
self.dx_prev = np.array(dx0, dtype=float)
return x
if x.shape != self.data_shape:
raise ValueError("Unexpected data shape")
t_e = t - self.t_prev
# The filtered derivative of the signal
a_d = smoothing_factor(t_e, self.d_cutoff)
dx = (x - self.x_prev) / t_e
dx_hat = self.smoothing_fn(a_d, dx, self.dx_prev)
# The filtered signal
cutoff = self.min_cutoff + self.beta * np.abs(dx_hat)
a = smoothing_factor(t_e, cutoff)
x_hat = self.smoothing_fn(a, x, self.x_prev)
# Memorize the previous values
self.x_prev = x_hat
self.dx_prev = dx_hat
self.t_prev = t
return x_hat
class LPRotationFilter:
"""https://github.com/Dingry/bunny_teleop_server/blob/main/bunny_teleop_server/utils/robot_utils.py"""
def __init__(self, alpha):
self.alpha = alpha
self.is_init = False
self.y = None
def next(self, x: np.ndarray):
assert x.shape == (4,)
if not self.is_init:
self.y = x
self.is_init = True
return self.y.copy()
self.y = rotational_exponential_smoothing(self.alpha, x, self.y)
return self.y.copy()
def next_mat(self, x: np.ndarray):
"""take and return rotation matrix instead of quat"""
assert x.shape == (3, 3) or x.shape == (4, 4)
if x.shape == (4, 4):
x = x[:3, :3]
x = R.from_matrix(x).as_quat()
next_x_quat = self.next(x)
return R.from_quat(next_x_quat).as_matrix()
def reset(self):
self.y = None
self.is_init = False
调用例子：
初始化：
self.left_positiion_filter = OneEuroFilter(
min_cutoff=self.config.position_filter.min_cutoff,
beta=self.config.position_filter.beta,
)
self.left_orientation_filter = LPRotationFilter(self.config.orientation_filter.alpha)
调用：
xyzquat = pin.SE3ToXYZQUAT(pose)
t = time.time()
xyzquat[:3] = self.left_positiion_filter.next(t, xyzquat[:3])
xyzquat[3:] = self.left_orientation_filter.next(xyzquat[3:])
扫一扫
937
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
