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

矩阵方式:
在这里插入图片描述
下面是代码:

#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);

函数方式:
在这里插入图片描述
根据三角函数的特性,可以进行一下简化:
在这里插入图片描述

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

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);

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:/a/532496.html

如若内容造成侵权/违法违规/事实不符,请联系我们进行投诉反馈qq邮箱809451989@qq.com,一经查实,立即删除!

相关文章

解读POP3:电子邮件查看必备技巧揭秘

在您点击阅读时&#xff0c;是否曾想过您是如何如此轻松地查看电子邮件的&#xff1f;对我们来说&#xff0c;这听起来可能只是几秒钟的加载时间&#xff0c;但实际上幕后发生了许多事情。邮局协议&#xff08;POP3&#xff09;是一种应用层协议&#xff0c;电子邮件客户端使用…

基于Android的记单词App系统的设计与实现

博主介绍&#xff1a;✌IT徐师兄、7年大厂程序员经历。全网粉丝15W、csdn博客专家、掘金/华为云//InfoQ等平台优质作者、专注于Java技术领域和毕业项目实战✌ &#x1f345;文末获取源码联系&#x1f345; &#x1f447;&#x1f3fb; 精彩专栏推荐订阅&#x1f447;&#x1f3…

Linkedin领英封号原因是什么?如何养号?

领英作为全球最大的职场社交平台&#xff0c;用户总数已超过8亿&#xff0c;覆盖200多个国家和地区&#xff0c;中国会员总数也已经累计超过5700万&#xff0c;庞大的基数使得他迅速成为跨境业务员建立形象&#xff0c;拓展人脉&#xff0c;开发客户的重要渠道。“领英职场”的…

C语言面试题之检查二叉树平衡性

检查二叉树平衡性 实例要求 1、实现一个函数&#xff0c;检查二叉树是否平衡&#xff1b;2、在这个问题中&#xff0c;平衡树的定义如下&#xff1a;任意一个节点&#xff0c;其两棵子树的高度差不超过 1&#xff1b; 示例 1: 给定二叉树 [3,9,20,null,null,15,7]3/ \9 20/…

【PyQt5篇】和子线程进行通信

文章目录 &#x1f354;使用QtDesigner进行设计&#x1f6f8;和子线程进行通信&#x1f388;运行结果 &#x1f354;使用QtDesigner进行设计 我们首先使用QtDesigner设计界面 得到代码login.ui <?xml version"1.0" encoding"UTF-8"?> <ui …

如何保证消息不丢失?——使用rabbitmq的死信队列!

如何保证消息不丢失?——使用rabbitmq的死信队列&#xff01; 1、什么是死信 在 RabbitMQ 中充当主角的就是消息&#xff0c;在不同场景下&#xff0c;消息会有不同地表现。 死信就是消息在特定场景下的一种表现形式&#xff0c;这些场景包括&#xff1a; 消息被拒绝访问&am…

c/c++函数: strtok() ,strtok_s()

概述 函数的原型&#xff1a; char* strtok : strtok (char* _String, char const* _Delimiter); char* strtok_s: strtok_s( char* _String, char const* _Delimiter, char** _Context);函数的参数: _String : 传入一个字符串 _Delimiter: 传入一个字符字…

【canvas】canvas基础使用(四):线型的设置

简言 学习如何使用canvas来设置线形。 线型的方法和属性 使用canvas经常会和线段打交道&#xff0c;下面是设置线型的常用属性和方法。 lineWidth 线宽 CanvasRenderingContext2D.lineWidth 是 Canvas 2D API 设置线段厚度的属性&#xff08;即线段的宽度&#xff09;。 线…

VR紧急情况模拟|V R体验中心加盟|元宇宙文旅

通过VR技术实现紧急情况模拟&#xff0c;提升安全应急能力&#xff01; 简介&#xff1a;面对突发紧急情况&#xff0c;如火灾、地震、交通事故等&#xff0c;正确的反应和应对能够有效减少伤害和损失。为了提高人们在紧急情况下的应急能力&#xff0c;我们借助先进的虚拟现实…

全国水科技大会 免费征集《水环境治理减污降碳协同增效示范案例》

申报时间截止到2024年4月15日&#xff0c;请各单位抓紧申报&#xff0c;申报条件及申报表请联系&#xff1a;13718793867 围绕水环境治理减污降碳协同增效领域&#xff0c;以资源化、生态化和可持续化为导向&#xff0c;面向生态、流城、城市、农村、工业园区、电力、石化、钢…

寄快递的省钱小妙招,看看你能知道多少

首先就是从包裹的重量上和体积上&#xff0c;我们都知道快递员上门取件都是需要称重的&#xff0c;我们能做的就是尽量压缩包裹的体积来减少快递的运费价格。然后是使用自己的包装袋来打包行李&#xff0c;快递员的袋子也是需要另外花费的。对于一些不容易损坏的货物来说&#…

scan纯享代码 java

scan纯享代码 java 1 scan用法2 next3 nextLine 1 scan用法 在录入中间有回车的字符串的时候&#xff0c;不要使用next&#xff08;&#xff09;和nextLine&#xff08;&#xff09;的配合&#xff01;&#xff01; scan用法 Scanner scannernew Scanner(System.in); String…

了解单链表

27. 移除元素 - 力扣&#xff08;LeetCode&#xff09; 思路一&#xff1a; 创建新的数组&#xff0c;遍历原数组&#xff0c;将不为val的值放到新数组当中。空间复杂度不为O(1) 思路二&#xff1a;双指针法 我们设置两个指针src&#xff08;源数据&#xff09;和dst&#xf…

STM32G030F6P6 HSE时钟不能使用无源晶振,只能使用有源晶振!

STM32G030F6P6 HSE时钟不能使用无源晶振&#xff0c;只能使用有源晶振。 参见STM32CubeMX配置 使能RCC中 BYPASS CLOCK SOURCE后只有一个 PC14引脚。 查手册中 5.2.1 HSE clock章节 部分引脚少的封装&#xff0c;HSE时钟只有 OSC-IN&#xff0c;因此只能使用有源晶振 查Data…

mybatis-动态sql

动态sql 1、if标签2、where标签3、trim标签4、set标签5、choose when otherwise6 、foreach6.1 用in来删除6.2 用or来删除6.3 批量添加 7、 sql标签与include标签 1、if标签 需求&#xff1a;多条件查询。 可能的条件包括&#xff1a;品牌&#xff08;brand&#xff09;、指导…

SQL注入sqli_labs靶场第二题

解题思路与第一题相同 ?id1 and 11 和?id1 and 12进行测试如果11页面显示正常和原页面一样&#xff0c;并且12页面报错或者页面部分数据显示不正常&#xff0c;那么可以确定此处为数字型注入。 联合查询&#xff1a; 猜解列名数量&#xff1a;3 ?id1 order by 4 判断回显…

20240410解决OK3588-C的核心板刷机之后无法启动的问题

20240410解决OK3588-C的核心板刷机之后无法启动的问题 2024/4/10 19:38 1、编译OK3588的LINUX/Buildroot&#xff1f;forlinxubuntu: ~/3588/OK3588_Linux_fs$ sudo ./build.sh BoardConfig-linuxfs-ok3588.mk 2、进行全编译 forlinxubuntu: ~/3588/OK3588_Linux_fs$ sudo ./bu…

ArrayList中多线程的不安全问题

ArrayList中的不安全问题 正常的输出 List<String> list Arrays.asList("1","2","3"); list.forEach(System.out::println);为什么可以这样输出&#xff0c;是一种函数是接口&#xff0c;我们先过个耳熟 Arrys.asList是返回一个ArrayL…

ruoyi-nbcio-plus基于vue3的flowable的自定义业务提交申请组件的升级修改

更多ruoyi-nbcio功能请看演示系统 gitee源代码地址 前后端代码&#xff1a; https://gitee.com/nbacheng/ruoyi-nbcio 演示地址&#xff1a;RuoYi-Nbcio后台管理系统 http://122.227.135.243:9666/ 更多nbcio-boot功能请看演示系统 gitee源代码地址 后端代码&#xff1a…

nandgame中的Tokenize(标记化)

题目说明&#xff1a; "Tokenize" "标记化"标记器预先配置为识别数字和符号 。请配置标记器以额外识别符号减号 - 和括号 ( 和 )。您可以编辑源代码区域中的代码以测试它的标记化。level help 我们将构建一种高级编程语言。 高级语言具有更加人性化和灵…