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

矩阵方式:

下面是代码:

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);
相关推荐
kyle~2 小时前
机器人传感器系统---时间戳对齐
机器人
淮北49415 小时前
ros调试工具foxglove使用指南三:在3d空间写写画画(Panel->3D ->Scene entity)
python·学习·3d·机器人
AntResearch19 小时前
ICLR 2025 Spotlight:让机器人实现「自主进化」,蚂蚁数科、清华提出具身协同框架 BodyGen
机器人
东雁西飞21 小时前
MATLAB 控制系统设计与仿真 - 33
开发语言·算法·matlab·机器人·自动控制
郭涤生1 天前
第二章:ROS架构_《ROS机器人开发实践》_notes
架构·机器人
MobiCetus1 天前
【MachineLearning】生成对抗网络 (GAN)
linux·人工智能·python·深度学习·神经网络·生成对抗网络·机器人
古希腊掌握嵌入式的神1 天前
[ROS]ROS系统是如何协调工作机器人
机器人·ros
硅谷秋水1 天前
DataPlatter:利用最少成本数据提升机器人操控的泛化能力
人工智能·深度学习·计算机视觉·语言模型·机器人
Mr.Winter`1 天前
深度强化学习 | 基于优先级经验池的DQN算法(附Pytorch实现)
人工智能·pytorch·神经网络·机器学习·机器人·强化学习
GIS数据转换器2 天前
在机器人和无人机时代,测绘人的出路在哪里?
大数据·人工智能·信息可视化·机器人·自动驾驶·汽车·无人机