矩陣方式：
下面是代碼：
#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); // 創(chuàng)建旋轉(zhuǎn)矩陣
Eigen::Vector2d d(pose.x, pose.y); // 創(chuàng)建平移向量
for (const auto& point : pc_tar) {
Eigen::Vector2d local_point(point.x, point.y);
Eigen::Vector2d world_point = R * local_point + d; // 進(jìn)行坐標(biāo)轉(zhuǎn)換
tmp_pc.push_back(PointPCL(world_point.x(), world_point.y(), point.z));
}
pcl::copyPointCloud(tmp_pc, pc_org);
}
// 在調(diào)用此函數(shù)時(shí)：
transLocalToWorldCloudWith2dPose(pc_tar, pose, pc_org);
函數(shù)方式：
根據(jù)三角函數(shù)的特性，可以進(jìn)行一下簡化：
文章來源：http://www.zghlxwxcb.cn/news/detail-852737.html
下面是簡化前的代碼示例：文章來源地址http://www.zghlxwxcb.cn/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);
}
// 在調(diào)用此函數(shù)時(shí)：
transLocalToWorldCloudWith2dPose(pc_tar, pose, pc_org);
到了這里，關(guān)于機(jī)器人坐標(biāo)系轉(zhuǎn)換從局部坐標(biāo)系轉(zhuǎn)換到世界坐標(biāo)系的文章就介紹完了。如果您還想了解更多內(nèi)容，請?jiān)谟疑辖撬阉鱐OY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！
