在点云处理过程当中,常常需要计算某一点的点云位姿,为了能够更加直观的展示姿态信息,很多时候需要我们将姿态信息能够在点云上以坐标轴的形式显示出来。以下分享的就是在已知点位和姿态信息的情况下生成坐标系的方法。
原理很简单,在三维空间内,已知一点和该点的向量,我们才能知道该点的位姿。所以,一般情况下我们会使用一个4*4的矩阵表示一个空间点。
上图展示的就是矩阵中代表方向的3*3矩阵,分别对应在三个轴上的值。另外,还有点的位置信息(x, y, z)。拼在一起就是一个3*4的矩阵,一般也可以在第4行填充(0, 0, 0, 1),形成一个4*4的矩阵。这个就是看怎么方便怎么表示,最终目的就是同时拿到点坐标和RT姿态。
这也可以用在,已知点和法向,沿着该法向筛选点云的一个方法。具体可以之后分享。
接下来就是实现一个轴的实现方式:
//设置每个点之间的间隔距离
double distance = 2.0;
//申明一个法向向量,并赋值
Eigen::Vector3d direction(0, 0, 1);
//申明一个三维点,并赋值,该点作为起始点
Eigen::Vector3d point(0, 0, 0);
//申明一个点云
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_Line(new pcl::PointCloud<pcl::PointXYZ>);
//将起始点push进去
cloud_Line->push_back(pcl::PointXYZ(point[0], point[1], point[2]));
//开始循环并将点push到点云内
for (int i = 1; i < 150; ++i) {
Eigen::Vector3d new_point = point_down + i * distance * direction;
cloud_Line->push_back(pcl::PointXYZ(new_point[0], new_point[1], new_point[2]));
}
//保存点云,至此一个轴生成完成
pcl::io::savePLYFileBinary("cloud_Line.ply", *cloud_Line);
每行代码我都做了对应的注释,希望能够有用。
草稿代码可能不是那么严谨,但运行起来问题不到。