pcl 生成线段点云
pcl官方有生成球体、圆柱体、圆锥体的相关函数,似乎没有生成线段的函数,正好有需要,所以自己写了两个,分别是根据数量和步长生成。
原理
原理很简单,已知两个三维点p1,p2,求出方向向量和向量p1p2的模,再根据设置的点的数量和步长添加进点云。
// 根据数量
void creatseg(pcl::PointXYZ& p1, pcl::PointXYZ& p2, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, int num) {
Eigen::Vector3d k(p1.x - p2.x, p1.y - p2.y, p1.z - p2.z);
if (num<1){
print_highlight("点的数量应为大于0的整数");
}
else {
double delt = k.norm() / num;
k = k / k.norm();
cloud->points.resize(num);
for (auto i = 0; i < num; ++i) {
cloud->points[i].x = p2.x + delt * i * k[0];
cloud->points[i].y = p2.y + delt * i * k[1];
cloud->points[i].z = p2.z + delt * i * k[2];
}
cloud->push_back(p1);
cloud->push_back(p2);
}
}
// 根据步长
void creatsegment(pcl::PointXYZ& p1, pcl::PointXYZ& p2, pcl::PointCloud<pcl::PointXYZ> ::Ptr cloud, double delt) {
Eigen::Vector3d k(p1.x - p2.x, p1.y - p2.y, p1.z - p2.z);
int num = k.norm() / delt;
k = k / k.norm();
cloud->points.resize(num);
for (auto i = 0; i < num; ++i) {
cloud->points[i].x = p2.x + delt * i * k[0];
cloud->points[i].y = p2.y + delt * i * k[1];
cloud->points[i].z = p2.z + delt * i * k[2];
}
cloud->push_back(p1);
cloud->push_back(p2);
}
测试代码如下
#include <pcl/PCLPointCloud2.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/uniform_sampling.h>
#include <pcl/console/print.h>
#include <pcl/console/parse.h>
#include <pcl/console/time.h>
using namespace std;
using namespace pcl;
using namespace pcl::io;
using namespace pcl::console;
void creatseg(pcl::PointXYZ& p1, pcl::PointXYZ& p2, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, int num) {
Eigen::Vector3d k(p1.x - p2.x, p1.y - p2.y, p1.z - p2.z);
if (num<1){
print_highlight("点的数量应为大于0的整数");
}
else {
double delt = k.norm() / num;
k = k / k.norm();
cloud->points.resize(num);
for (auto i = 0; i < num; ++i) {
cloud->points[i].x = p2.x + delt * i * k[0];
cloud->points[i].y = p2.y + delt * i * k[1];
cloud->points[i].z = p2.z + delt * i * k[2];
}
cloud->push_back(p1);
cloud->push_back(p2);
}
}
void creatsegment(pcl::PointXYZ& p1, pcl::PointXYZ& p2, pcl::PointCloud<pcl::PointXYZ> ::Ptr cloud, double delt) {
Eigen::Vector3d k(p1.x - p2.x, p1.y - p2.y, p1.z - p2.z);
int num = k.norm() / delt;
k = k / k.norm();
cloud->points.resize(num);
for (auto i = 0; i < num; ++i) {
cloud->points[i].x = p2.x + delt * i * k[0];
cloud->points[i].y = p2.y + delt * i * k[1];
cloud->points[i].z = p2.z + delt * i * k[2];
}
cloud->push_back(p1);
cloud->push_back(p2);
}
int main() {
pcl::PointXYZ P1(12, 2.3, 7.89), P2(0, 0, 0);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
creatseg(P1, P2, cloud, 10000);
pcl::io::savePCDFile("line.pcd",*cloud);
creatsegment(P1, P2, cloud, 0.0001);
pcl::io::savePCDFile("segment.pcd", *cloud);
}
生成的线段如下图。