矩阵方式:
下面是代码:
#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);