矩阵方式:
下面是代码:
#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);
函数方式:
根据三角函数的特性,可以进行一下简化:文章来源:https://www.toymoban.com/news/detail-852737.html
下面是简化前的代码示例:文章来源地址https://www.toymoban.com/news/detail-852737.html
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);
到了这里,关于机器人坐标系转换从局部坐标系转换到世界坐标系的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!