355 lines
11 KiB
C++
355 lines
11 KiB
C++
#include "mypcl.h"
|
||
|
||
namespace ai_matrix
|
||
{
|
||
MyPcl *MyPcl::ins = nullptr;
|
||
MyPcl::GarbageCollector MyPcl::gc;
|
||
std::mutex MyPcl::mx;
|
||
|
||
MyPcl *MyPcl::getins()
|
||
{
|
||
//双层锁，确保线程安全
|
||
if (ins == nullptr)
|
||
{
|
||
std::lock_guard<std::mutex> guard(mx); //防止异常发生不能解锁
|
||
if (ins == nullptr)
|
||
{
|
||
ins = new MyPcl();
|
||
}
|
||
}
|
||
return ins;
|
||
}
|
||
|
||
void MyPcl::compressPCD(const pcl::PointCloud<pcl::PointXYZRGB> &cloud, std::stringstream &compressedData)
|
||
{
|
||
bool showStatistics = false; //设置在标准设备上输出打印出压缩结果信息
|
||
// 压缩选项详见 /io/include/pcl/compression/compression_profiles.h
|
||
pcl::io::compression_Profiles_e compressionProfile = pcl::io::LOW_RES_OFFLINE_COMPRESSION_WITH_COLOR;
|
||
// 初始化压缩与解压缩对象，其中压缩对象需要设定压缩参数选项，解压缩按照数据源自行判断
|
||
pcl::io::OctreePointCloudCompression<pcl::PointXYZRGB> *PointCloudEncoder;
|
||
PointCloudEncoder = new pcl::io::OctreePointCloudCompression<pcl::PointXYZRGB>(compressionProfile, showStatistics);
|
||
PointCloudEncoder->encodePointCloud(cloud.makeShared(), compressedData); // 压缩点云
|
||
delete (PointCloudEncoder);
|
||
}
|
||
|
||
void MyPcl::insert_plane_line(pcl::PointCloud<pcl::PointXYZRGB> &line, float x_min, float x_max, float y, float z, unsigned char r, unsigned char g, unsigned char b, float step)
|
||
{
|
||
int num = 0;
|
||
for (float x = x_min; x <= x_max; x += step)
|
||
{
|
||
pcl::PointXYZRGB pt;
|
||
pt.x = x;
|
||
pt.y = y;
|
||
pt.z = z;
|
||
pt.r = r;
|
||
pt.g = g;
|
||
pt.b = b;
|
||
|
||
line.points.push_back(pt);
|
||
num++;
|
||
}
|
||
|
||
line.width += num;
|
||
line.height = 1;
|
||
}
|
||
|
||
void MyPcl::insert_load_line(pcl::PointCloud<pcl::PointXYZRGB> &line, float x_min, float x_max, float y_min, float y_max, float z, unsigned char r, unsigned char g, unsigned char b, float step)
|
||
{
|
||
int num = 0;
|
||
for (float x = x_min; x <= x_max; x += step)
|
||
{
|
||
pcl::PointXYZRGB pt;
|
||
pt.x = x;
|
||
pt.y = y_min;
|
||
pt.z = z;
|
||
pt.r = r;
|
||
pt.g = g;
|
||
pt.b = b;
|
||
|
||
line.points.push_back(pt);
|
||
num++;
|
||
}
|
||
|
||
for (float x = x_min; x <= x_max; x += step)
|
||
{
|
||
pcl::PointXYZRGB pt;
|
||
pt.x = x;
|
||
pt.y = y_max;
|
||
pt.z = z;
|
||
pt.r = r;
|
||
pt.g = g;
|
||
pt.b = b;
|
||
|
||
line.points.push_back(pt);
|
||
num++;
|
||
}
|
||
|
||
for (float y = y_min; y <= y_max; y += step)
|
||
{
|
||
pcl::PointXYZRGB pt;
|
||
pt.x = x_min;
|
||
pt.y = y;
|
||
pt.z = z;
|
||
pt.r = r;
|
||
pt.g = g;
|
||
pt.b = b;
|
||
|
||
line.points.push_back(pt);
|
||
num++;
|
||
}
|
||
|
||
for (float y = y_min; y <= y_max; y += step)
|
||
{
|
||
pcl::PointXYZRGB pt;
|
||
pt.x = x_max;
|
||
pt.y = y;
|
||
pt.z = z;
|
||
pt.r = r;
|
||
pt.g = g;
|
||
pt.b = b;
|
||
|
||
line.points.push_back(pt);
|
||
num++;
|
||
}
|
||
|
||
line.width += num;
|
||
line.height = 1;
|
||
}
|
||
|
||
void MyPcl::insert_bound_line(pcl::PointCloud<pcl::PointXYZRGB> &line, float y_min, float y_max, float z_min, float z_max, float x, unsigned char r, unsigned char g, unsigned char b, float step)
|
||
{
|
||
int num = 0;
|
||
|
||
for (float y = y_min; y <= y_max; y += step)
|
||
{
|
||
pcl::PointXYZRGB pt;
|
||
pt.x = x;
|
||
pt.y = y;
|
||
pt.z = z_max;
|
||
pt.r = r;
|
||
pt.g = g;
|
||
pt.b = b;
|
||
|
||
line.points.push_back(pt);
|
||
num++;
|
||
}
|
||
|
||
for (float z = z_min; z <= z_max; z += step)
|
||
{
|
||
pcl::PointXYZRGB pt;
|
||
pt.x = x;
|
||
pt.y = y_min;
|
||
pt.z = z;
|
||
pt.r = r;
|
||
pt.g = g;
|
||
pt.b = b;
|
||
|
||
line.points.push_back(pt);
|
||
num++;
|
||
}
|
||
|
||
for (float z = z_min; z <= z_max; z += step)
|
||
{
|
||
pcl::PointXYZRGB pt;
|
||
pt.x = x;
|
||
pt.y = y_max;
|
||
pt.z = z;
|
||
pt.r = r;
|
||
pt.g = g;
|
||
pt.b = b;
|
||
|
||
line.points.push_back(pt);
|
||
num++;
|
||
}
|
||
|
||
line.width += num;
|
||
line.height = 1;
|
||
}
|
||
|
||
void MyPcl::insert_object_line(pcl::PointCloud<pcl::PointXYZRGB> &line, float x_min, float x_max, float y, float z_min, float z_max, unsigned char r, unsigned char g, unsigned char b, float step)
|
||
{
|
||
int num = 0;
|
||
|
||
for (float x = x_min; x <= x_max; x += step)
|
||
{
|
||
pcl::PointXYZRGB pt;
|
||
pt.x = x;
|
||
pt.y = y;
|
||
pt.z = z_min;
|
||
pt.r = r;
|
||
pt.g = g;
|
||
pt.b = b;
|
||
|
||
line.points.push_back(pt);
|
||
num++;
|
||
}
|
||
|
||
for (float x = x_min; x <= x_max; x += step)
|
||
{
|
||
pcl::PointXYZRGB pt;
|
||
pt.x = x;
|
||
pt.y = y;
|
||
pt.z = z_max;
|
||
pt.r = r;
|
||
pt.g = g;
|
||
pt.b = b;
|
||
|
||
line.points.push_back(pt);
|
||
num++;
|
||
}
|
||
|
||
for (float z = z_min; z <= z_max; z += step)
|
||
{
|
||
pcl::PointXYZRGB pt;
|
||
pt.x = x_min;
|
||
pt.y = y;
|
||
pt.z = z;
|
||
pt.r = r;
|
||
pt.g = g;
|
||
pt.b = b;
|
||
|
||
line.points.push_back(pt);
|
||
num++;
|
||
}
|
||
|
||
for (float z = z_min; z <= z_max; z += step)
|
||
{
|
||
pcl::PointXYZRGB pt;
|
||
pt.x = x_max;
|
||
pt.y = y;
|
||
pt.z = z;
|
||
pt.r = r;
|
||
pt.g = g;
|
||
pt.b = b;
|
||
|
||
line.points.push_back(pt);
|
||
num++;
|
||
}
|
||
|
||
line.width += num;
|
||
line.height = 1;
|
||
}
|
||
|
||
void MyPcl::coords_convert(const pcl::PointCloud<pcl::PointXYZ> &cloud_in, pcl::PointCloud<pcl::PointXYZ> &cloud_out, const Eigen::Matrix4f &tf, float thetaz)
|
||
{
|
||
Eigen::Matrix4f tf_use;
|
||
|
||
//沿z轴旋转
|
||
Eigen::Matrix4f tfz = Eigen::Matrix4f::Identity();
|
||
tfz(0, 0) = cos(thetaz);
|
||
tfz(0, 1) = -sin(thetaz);
|
||
tfz(1, 0) = sin(thetaz);
|
||
tfz(1, 1) = cos(thetaz);
|
||
|
||
//y坐标乘以-1
|
||
Eigen::Matrix4f tfy = Eigen::Matrix4f::Identity();
|
||
tfy(0, 0) = -1;
|
||
|
||
tf_use = tfz * tfy * tf; //一起旋转，效率更高
|
||
|
||
pcl::transformPointCloud(cloud_in, cloud_out, tf_use);
|
||
}
|
||
|
||
void MyPcl::coords_convert(const pcl::PointCloud<pcl::PointXYZ> &cloud_in, pcl::PointCloud<pcl::PointXYZ> &cloud_out, float thetax, float thetay, float thetaz, float radarz, bool reverse)
|
||
{
|
||
//绕x轴旋转
|
||
Eigen::Matrix4f tfx = Eigen::Matrix4f::Identity();
|
||
tfx(1, 1) = cos(thetax);
|
||
tfx(1, 2) = -sin(thetax);
|
||
tfx(2, 1) = sin(thetax);
|
||
tfx(2, 2) = cos(thetax);
|
||
|
||
//绕y轴旋转
|
||
Eigen::Matrix4f tfy = Eigen::Matrix4f::Identity();
|
||
tfy(0, 0) = cos(thetay);
|
||
tfy(0, 2) = sin(thetay);
|
||
tfy(2, 0) = -sin(thetay);
|
||
tfy(2, 2) = cos(thetay);
|
||
|
||
//沿z轴旋转
|
||
Eigen::Matrix4f tfz = Eigen::Matrix4f::Identity();
|
||
tfz(0, 0) = cos(thetaz);
|
||
tfz(0, 1) = -sin(thetaz);
|
||
tfz(1, 0) = sin(thetaz);
|
||
tfz(1, 1) = cos(thetaz);
|
||
|
||
Eigen::Matrix4f tf_use = tfz * tfy * tfx; //一起旋转，效率更高
|
||
tf_use(2, 3) = radarz; //雷达高度
|
||
|
||
Eigen::Matrix4f tf_use_inv = tf_use.inverse(); //求逆矩阵
|
||
|
||
if (reverse)
|
||
{
|
||
pcl::transformPointCloud(cloud_in, cloud_out, tf_use_inv);
|
||
}
|
||
else
|
||
{
|
||
pcl::transformPointCloud(cloud_in, cloud_out, tf_use);
|
||
}
|
||
}
|
||
|
||
void MyPcl::cube_cloud(const pcl::PointCloud<pcl::PointXYZ> &cloud_in, std::vector<int> &indices, const Eigen::Vector3f &min, const Eigen::Vector3f &max)
|
||
{
|
||
//pcl::CropBox<pcl::PointXYZ> crop;
|
||
//crop.setMin(Eigen::Vector4f(min(0), min(1), min(2), 1.0));
|
||
//crop.setMax(Eigen::Vector4f(max(0), max(1), max(2), 1.0));
|
||
//crop.setInputCloud(cloud_in.makeShared());
|
||
//crop.setNegative(false); //默认为false，代表取立方体内的点
|
||
//crop.filter(indices);
|
||
for(int i = 0 ; i < cloud_in.points.size(); i ++)
|
||
{
|
||
if (min[0] < cloud_in.points[i].x && cloud_in.points[i].x < max[0]
|
||
&& min[1] < cloud_in.points[i].y && cloud_in.points[i].y < max[1]
|
||
&& min[2] < cloud_in.points[i].z && cloud_in.points[i].z < max[2])
|
||
{
|
||
indices.push_back(i);
|
||
}
|
||
}
|
||
}
|
||
|
||
void MyPcl::cube_cloud(const pcl::PointCloud<pcl::PointXYZ> &cloud_in, pcl::PointCloud<pcl::PointXYZ> &cloud_out, const Eigen::Vector3f &min, const Eigen::Vector3f &max)
|
||
{
|
||
//pcl::CropBox<pcl::PointXYZ> crop;
|
||
//crop.setMin(Eigen::Vector4f(min(0), min(1), min(2), 1.0));
|
||
//crop.setMax(Eigen::Vector4f(max(0), max(1), max(2), 1.0));
|
||
//crop.setInputCloud(cloud_in.makeShared());
|
||
//crop.setNegative(false); //默认为false，代表取立方体内的点
|
||
//crop.filter(cloud_out);
|
||
|
||
cloud_out.height = 1; //无序点云
|
||
cloud_out.is_dense = false; //加上，防止出错
|
||
for (int i = 0; i < cloud_in.points.size(); i++)
|
||
{
|
||
if (min[0] < cloud_in.points[i].x && cloud_in.points[i].x < max[0]
|
||
&& min[1] < cloud_in.points[i].y && cloud_in.points[i].y < max[1]
|
||
&& min[2] < cloud_in.points[i].z && cloud_in.points[i].z < max[2])
|
||
{
|
||
cloud_out.points.push_back(cloud_in.points[i]);
|
||
}
|
||
}
|
||
}
|
||
|
||
void MyPcl::cube_cloud(const pcl::PointCloud<pcl::PointXYZ> &cloud_in, pcl::PointCloud<pcl::PointXYZ> &cloud_out, std::vector<int> &indices, const Eigen::Vector3f &min, const Eigen::Vector3f &max)
|
||
{
|
||
//pcl::CropBox<pcl::PointXYZ> crop;
|
||
//crop.setMin(Eigen::Vector4f(min(0), min(1), min(2), 1.0));
|
||
//crop.setMax(Eigen::Vector4f(max(0), max(1), max(2), 1.0));
|
||
//crop.setInputCloud(cloud_in.makeShared());
|
||
//crop.setNegative(false); //默认为false，代表取立方体内的点
|
||
//crop.filter(cloud_out);
|
||
//crop.filter(indices);
|
||
cloud_out.height = 1; //无序点云
|
||
cloud_out.is_dense = false; //加上，防止出错
|
||
for (int i = 0; i < cloud_in.points.size(); i++)
|
||
{
|
||
if (min[0] < cloud_in.points[i].x && cloud_in.points[i].x < max[0]
|
||
&& min[1] < cloud_in.points[i].y && cloud_in.points[i].y < max[1]
|
||
&& min[2] < cloud_in.points[i].z && cloud_in.points[i].z < max[2])
|
||
{
|
||
cloud_out.points.push_back(cloud_in.points[i]);
|
||
indices.push_back(i);
|
||
}
|
||
}
|
||
}
|
||
}
|