机器人坐标系转换从局部坐标系转换到世界坐标系

奋进的大脑袋2024-04-14 18:45

矩阵方式：

下面是代码：

cpp 复制代码

#include <Eigen/Dense>

static void transLocalToWorldCloudWith2dPose(const PointCloud &pc_tar, const QPose3f &pose, PointCloud &pc_org) {
    if (pc_tar.empty())
        return;

    PointCloud tmp_pc;
    Eigen::Rotation2Dd R(-pose.yaw);  // 创建旋转矩阵
    Eigen::Vector2d d(pose.x, pose.y);  // 创建平移向量

    for (const auto& point : pc_tar) {
        Eigen::Vector2d local_point(point.x, point.y);
        Eigen::Vector2d world_point = R * local_point + d;  // 进行坐标转换

        tmp_pc.push_back(PointPCL(world_point.x(), world_point.y(), point.z));
    }

    pcl::copyPointCloud(tmp_pc, pc_org);
}

// 在调用此函数时：
transLocalToWorldCloudWith2dPose(pc_tar, pose, pc_org);

函数方式：

根据三角函数的特性，可以进行一下简化：

下面是简化前的代码示例：

cpp 复制代码

static void transLocalToWorldCloudWith2dPose(const PointCloud &pc_tar, const QPose3f &pose, PointCloud &pc_org) {
    if (pc_tar.empty())
        return;

    PointCloud tmp_pc;
    for (const auto& point : pc_tar) {
        double world_x;
        double world_y;
        double d_x = point.x * cos(-pose.yaw) + point.y * sin(-pose.yaw);
        double d_y = -point.x * sin(-pose.yaw) + point.y * cos(-pose.yaw);
        world_x = d_x + pose.x;
        world_y = d_y + pose.y;
        tmp_pc.push_back(PointPCL(world_x, world_y, point.z));
    }

    pcl::copyPointCloud(tmp_pc, pc_org);
}

// 在调用此函数时：
transLocalToWorldCloudWith2dPose(pc_tar, pose, pc_org);