激光雷达局部坐标转全局坐标代码问题排查求助

代码中的核心问题 #

1. 旋转计算时的变量覆盖错误
   你在计算全局坐标时，先修改了x的值，后续计算y时使用的是已经被修改过的x，这会导致旋转矩阵的计算逻辑完全错误。正确的做法是先保留局部坐标的原始值，再基于原始值进行旋转计算。

2. 位姿数据来源不一致
   getPos函数中同时从/odom获取位置，从/tf获取旋转，这两个数据的时间戳可能不匹配，会导致位姿信息不同步，最终转换结果偏差。应该统一从/odom的pose中提取位置和旋转（/odom的pose已经包含完整的位姿信息），或者通过TF的时间同步查询获取对应时刻的位姿。

3. 无效激光点过滤不完整
   激光雷达的ranges数据除了小于0.1的无效值，还可能包含inf（超出量程）或nan（无效数据），这些点也需要过滤，否则会引入异常坐标。

修正后的代码 #

import math
import numpy as np
from tf.transformations import euler_from_quaternion

def getPos(self):
    # 统一从/odom获取完整位姿，避免数据不同步
    pos = self.odom_data.pose.pose.position
    rotation = self.odom_data.pose.pose.orientation
    roll, pitch, yaw = euler_from_quaternion(
        (rotation.x, rotation.y, rotation.z, rotation.w)
    )
    return [pos.x, pos.y, yaw]

def getScan(self):
    scan = self.scan_data.ranges
    rpos = self.getPos()
    a_start = self.scan_data.angle_min
    a_increment = self.scan_data.angle_increment
    scan_xy = []
    yaw = rpos[2]
    cos_yaw = math.cos(yaw)
    sin_yaw = math.sin(yaw)
    robot_x = rpos[0]
    robot_y = rpos[1]

    for i in range(len(scan)):
        dist = scan[i]
        # 过滤所有无效值：小于最小量程、无穷大、非数值
        if dist < 0.1 or not np.isfinite(dist):
            continue
        # 计算激光点在雷达局部坐标系下的坐标
        local_x = dist * math.cos(a_start + i * a_increment)
        local_y = dist * math.sin(a_start + i * a_increment)
        # 先旋转再平移，使用原始局部坐标计算
        global_x = local_x * cos_yaw - local_y * sin_yaw + robot_x
        global_y = local_x * sin_yaw + local_y * cos_yaw + robot_y
        scan_xy.append([global_x, global_y])
    
    return np.array(scan_xy)

额外优化说明 #

• 提前计算cos_yaw和sin_yaw，避免循环中重复计算，提升效率
• 将位姿变量提前提取，让循环内逻辑更清晰
• 完善了无效点的过滤逻辑，避免异常数据干扰

内容的提问来源于stack exchange，提问作者Kaiwen Guo