旋转平移矩阵
点云摆正、点云配准等点云操作实质上是对点云进行旋转平移,使点云变换至目标位姿。PCL中使用的是矩阵乘列向量的计算方法。
下面分别给出了点A(x,y,z)绕X轴、Y轴、Z轴逆时针旋转角度θ的示意图与旋转矩阵(图1、图2、图3),以及“点云库Point Cloud Library”点云按照旋转矩阵实现的程序运行出的效果(图4、图5、图6)。
图1 绕X轴逆时针旋转角度θ
图2 绕Y轴逆时针旋转角度θ
图3 绕Z轴逆时针旋转角度θ
图4 绕X轴逆时针旋转90°
图5 绕Y轴逆时针旋转90°
图6 绕Z轴逆时针旋转90°
平移矩阵如下:
点A由(x,y,z)移动至点B(x+a,y+b,z+c),可视为点A沿着向量AB平移至点B,则向量AB=B-A=(x+a,y+b,z+c)-(x,y,z)=(a,b,c),即是平移矩阵的第四列。例如:将点云的质心移至坐标原点,点A为质心,点B为原点,则向量AB=(-cloudCentroid[0],-cloudCentroid[1],-cloudCentroid[2]),效果如图7所示。
图7 将点云质心移至坐标原点
评论中有人问点云怎么绕质心旋转,可能我的回答被“吞”了,导致大家看不到,我在这里回答一下。
由于PCL中点云旋转都是绕原点旋转的,你要绕质心旋转,则应该先将点云质心移至原点,绕原点旋转后,再将点云质心移回原来的位置,便达到了点云绕质心旋转的目的。
PCL中的旋转平移代码如下:
#include <pcl/io/pcd_io.h>
#include <pcl/common/transforms.h>
#include <pcl/visualization/cloud_viewer.h>
int main()
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::io::loadPCDFile("点云库字样.pcd", *cloud);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_transformed(new pcl::PointCloud<pcl::PointXYZ>);
Eigen::Vector4f cloudCentroid;
pcl::compute3DCentroid(*cloud, cloudCentroid);
Eigen::Matrix4f translation = Eigen::Matrix4f::Identity();
translation(0, 3) = -cloudCentroid[0];
translation(1, 3) = -cloudCentroid[1];
translation(2, 3) = -cloudCentroid[2];
pcl::transformPointCloud(*cloud, *cloud_transformed, translation);
pcl::visualization::PCLVisualizer viewer("PCLVisualizer");
viewer.initCameraParameters();
int v1(0);
viewer.createViewPort(0.0, 0.0, 0.5, 1.0, v1);
viewer.addCoordinateSystem(200, v1);
viewer.setBackgroundColor(128.0 / 255.0, 138.0 / 255.0, 135.0 / 255.0, v1);
viewer.addText("Cloud before transforming", 10, 10, "v1 test", v1);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> color(cloud, 0, 255, 0);
viewer.addPointCloud(cloud, color, "cloud", v1);
int v2(0);
viewer.createViewPort(0.5, 0.0, 1.0, 1.0, v2);
viewer.addCoordinateSystem(200, v2);
viewer.setBackgroundColor(128.0 / 255.0, 138.0 / 255.0, 135.0 / 255.0, v2);
viewer.addText("Cloud after transforming", 10, 10, "v2 test", v2);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> color_transformed(cloud_transformed, 0, 255, 0);
viewer.addPointCloud(cloud_transformed, color_transformed, "cloud_transformed", v2);
while (!viewer.wasStopped())
{
viewer.spinOnce(100);
boost::this_thread::sleep(boost::posix_time::microseconds(100000));
}
return 0;
}
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)