点云的机器坐标转世界坐标：原理与实现（C++/Python）

1. 问题背景

在机器人感知系统中，激光雷达采集的点云数据通常以传感器自身坐标系（机器坐标系）为参考。为了将这些数据用于建图、定位等任务，我们需要将其转换到全局的世界坐标系中。本文将详细介绍这一转换的数学原理，并提供C++和Python两种实现方式。

2. 数学原理

2.1 坐标系定义

• 机器坐标系：点云原始坐标(x₁,y₁,z₁)，原点在雷达中心
• 世界坐标系：全局固定坐标系

2.2 转换公式

转换过程分为旋转和平移两个步骤：

1. 旋转：应用由姿态角(yaw,pitch,roll)构成的旋转矩阵R
2. 平移：加上机器在世界坐标系中的位置(x,y)

数学表达式：P_world = R * P_machine + t

2.3 旋转矩阵计算

完整旋转矩阵（Z-Y-X顺序）：
R = R_z(yaw) R_y(pitch) R_x(roll)

展开后：
R = [
[cosθcosφ, cosθsinφsinψ-sinθcosψ, cosθsinφcosψ+sinθsinψ],
[sinθcosφ, sinθsinφsinψ+cosθcosψ, sinθsinφcosψ-cosθsinψ],
[-sinφ, cosφsinψ, cosφcosψ ]
]

3. C++实现

#include <Eigen/Dense>
#include <vector>

using namespace Eigen;

struct Pose {
    double x, y, z;
    double yaw, pitch, roll;
};

std::vector<Vector3d> transformPointCloud(
    const std::vector<Vector3d>& points,
    const Pose& pose) 
{
    // 计算旋转矩阵
    double cy = cos(pose.yaw), sy = sin(pose.yaw);
    double cp = cos(pose.pitch), sp = sin(pose.pitch);
    double cr = cos(pose.roll), sr = sin(pose.roll);

    Matrix3d R;
    R << cy*cp, cy*sp*sr - sy*cr, cy*sp*cr + sy*sr,
         sy*cp, sy*sp*sr + cy*cr, sy*sp*cr - cy*sr,
         -sp,    cp*sr,           cp*cr;

    // 应用变换
    std::vector<Vector3d> transformed;
    for (const auto& p : points) {
        Vector3d p_world = R * p;
        p_world += Vector3d(pose.x, pose.y, pose.z);
        transformed.push_back(p_world);
    }

    return transformed;
}

4. python实现

import numpy as np

def transform_point_cloud(points, pose):
    """
    :param points: Nx3 numpy数组
    :param pose: 包含x,y,z,yaw,pitch,roll的字典或对象
    :return: 转换后的Nx3点云
    """
    # 解包位姿
    x, y, z = pose['x'], pose['y'], pose['z']
    yaw, pitch, roll = pose['yaw'], pose['pitch'], pose['roll']

    # 计算旋转矩阵
    cy, sy = np.cos(yaw), np.sin(yaw)
    cp, sp = np.cos(pitch), np.sin(pitch)
    cr, sr = np.cos(roll), np.sin(roll)

    R = np.array([
        [cy*cp, cy*sp*sr - sy*cr, cy*sp*cr + sy*sr],
        [sy*cp, sy*sp*sr + cy*cr, sy*sp*cr - cy*sr],
        [-sp,    cp*sr,           cp*cr]
    ])

    # 应用变换
    points_rotated = np.dot(points, R.T)
    points_transformed = points_rotated + np.array([x, y, z])

    return points_transformed