企业官网建设流程全解析

影响网站收录的因数,wordpress最好的中文主题,营销推广的公司,网站正在建设中中文这段代码实现了一个 One Euro Filter#xff08;一欧元滤波器#xff09;。1. 原理简述One Euro Filter 是一种主要用于人机交互#xff08;如 VR 手柄追踪、光标移动、手势识别#xff09;的算法。它解决了一个核心矛盾#xff1a;抖动#xff08;Jitter#xff09;与延…这段代码实现了一个One Euro Filter一欧元滤波器。1. 原理简述One Euro Filter是一种主要用于人机交互如 VR 手柄追踪、光标移动、手势识别的算法。它解决了一个核心矛盾抖动Jitter与延迟Lag/Latency之间的权衡。问题如果你为了让信号平滑去抖动而使用强滤波会导致画面跟不上手的移动高延迟。如果你为了响应快低延迟而减少滤波画面会不停晃动高抖动。解决方案动态调整截止频率。当速度慢时手停在半空认为用户想精确定位增加滤波强度降低截止频率消除抖动。当速度快时手快速挥动认为用户关注的是速度和方向减少滤波强度提高截止频率消除延迟。import numpy as np class OneEuroFilter: def __init__( self, min_cutoff1.0, # 最小截止频率 (Hz)。值越小静止时越平滑抖动越少但迟滞感越强。 beta0.0, # 速度系数。值越大快速移动时响应越快延迟越低但可能引入高频噪声。 d_cutoff1.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, dx0None): 计算下一帧的过滤信号。 参数: 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, dtypefloat) # 记录当前值 # 初始化速度导数如果没有提供 dx0则默认为 0 if dx0 is None: self.dx_prev np.zeros_like(x) else: self.dx_prev np.array(dx0, dtypefloat) 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 False2. 原理解析这个类的核心原理结合了指数移动平均 (EMA)和四元数几何 (Quaternion Geometry)。A. 为什么要用这个类(One Euro Filter vs LP Filter)这只是一个简单的低通滤波器 (Low Pass Filter, LPF)它的 alpha 是固定的。对比上一条的 One Euro FilterOne 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_cutoff1.0, beta0.0, d_cutoff1.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, dx0None): 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, dtypefloat) if dx0 is None: self.dx_prev np.zeros_like(x) else: self.dx_prev np.array(dx0, dtypefloat) 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_cutoffself.config.position_filter.min_cutoff,betaself.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:])