346 lines
11 KiB
C++
346 lines
11 KiB
C++
#include "core/PointCloudProcessor.h"
|
||
#include <QDebug>
|
||
#include <vector>
|
||
#include <cmath>
|
||
|
||
#ifndef M_PI
|
||
#define M_PI 3.14159265358979323846
|
||
#endif
|
||
|
||
PointCloudProcessor::PointCloudProcessor(QObject *parent)
|
||
: QObject(parent)
|
||
, m_fx(1432.8957f)
|
||
, m_fy(1432.6590f)
|
||
, m_cx(637.5117f)
|
||
, m_cy(521.8720f)
|
||
, m_zScale(0.2f)
|
||
, m_imageWidth(1224)
|
||
, m_imageHeight(1024)
|
||
, m_totalPoints(1224 * 1024)
|
||
, m_platform(nullptr)
|
||
, m_device(nullptr)
|
||
, m_context(nullptr)
|
||
, m_queue(nullptr)
|
||
, m_program(nullptr)
|
||
, m_kernel(nullptr)
|
||
, m_depthBuffer(nullptr)
|
||
, m_xyzBuffer(nullptr)
|
||
, m_clInitialized(false)
|
||
{
|
||
}
|
||
|
||
PointCloudProcessor::~PointCloudProcessor()
|
||
{
|
||
cleanupOpenCL();
|
||
}
|
||
|
||
void PointCloudProcessor::setCameraIntrinsics(float fx, float fy, float cx, float cy)
|
||
{
|
||
m_fx = fx;
|
||
m_fy = fy;
|
||
m_cx = cx;
|
||
m_cy = cy;
|
||
}
|
||
|
||
void PointCloudProcessor::setZScaleFactor(float scale)
|
||
{
|
||
m_zScale = scale;
|
||
}
|
||
|
||
bool PointCloudProcessor::initializeOpenCL()
|
||
{
|
||
if (m_clInitialized) {
|
||
return true;
|
||
}
|
||
|
||
cl_int err;
|
||
qDebug() << "[OpenCL] Starting initialization...";
|
||
|
||
// 1. 获取平台
|
||
err = clGetPlatformIDs(1, &m_platform, nullptr);
|
||
if (err != CL_SUCCESS) {
|
||
emit errorOccurred("Failed to get OpenCL platform");
|
||
return false;
|
||
}
|
||
|
||
// 2. 获取设备（优先GPU）
|
||
err = clGetDeviceIDs(m_platform, CL_DEVICE_TYPE_GPU, 1, &m_device, nullptr);
|
||
if (err != CL_SUCCESS) {
|
||
qDebug() << "[OpenCL] GPU not available, using CPU";
|
||
err = clGetDeviceIDs(m_platform, CL_DEVICE_TYPE_CPU, 1, &m_device, nullptr);
|
||
if (err != CL_SUCCESS) {
|
||
emit errorOccurred("Failed to get OpenCL device");
|
||
return false;
|
||
}
|
||
}
|
||
|
||
// 3. 创建上下文
|
||
m_context = clCreateContext(nullptr, 1, &m_device, nullptr, nullptr, &err);
|
||
if (err != CL_SUCCESS) {
|
||
emit errorOccurred("Failed to create OpenCL context");
|
||
return false;
|
||
}
|
||
|
||
// 4. 创建命令队列
|
||
#if CL_TARGET_OPENCL_VERSION >= 200
|
||
cl_queue_properties props[] = { CL_QUEUE_ON_DEVICE_DEFAULT, 0 };
|
||
m_queue = clCreateCommandQueueWithProperties(m_context, m_device, props, &err);
|
||
#else
|
||
m_queue = clCreateCommandQueue(m_context, m_device, 0, &err);
|
||
#endif
|
||
if (err != CL_SUCCESS) {
|
||
emit errorOccurred("Failed to create command queue");
|
||
cleanupOpenCL();
|
||
return false;
|
||
}
|
||
|
||
qDebug() << "[OpenCL] Platform, device, context, and queue created successfully";
|
||
|
||
// 5. 创建并编译OpenCL程序
|
||
const char* kernelSource =
|
||
"__kernel void compute_xyz(__global const float* depth, "
|
||
"__global float* xyz, int width, int height, "
|
||
"float inv_fx, float inv_fy, float cx, float cy, float z_scale) { "
|
||
"int idx = get_global_id(0); "
|
||
"if (idx >= width * height) return; "
|
||
"int y = idx / width; "
|
||
"int x = idx % width; "
|
||
"float z = depth[idx] * z_scale; "
|
||
"xyz[idx*3] = (x - cx) * z * inv_fx; "
|
||
"xyz[idx*3+1] = (y - cy) * z * inv_fy; "
|
||
"xyz[idx*3+2] = z; "
|
||
"}";
|
||
|
||
m_program = clCreateProgramWithSource(m_context, 1, &kernelSource, nullptr, &err);
|
||
if (err != CL_SUCCESS) {
|
||
emit errorOccurred("Failed to create OpenCL program");
|
||
cleanupOpenCL();
|
||
return false;
|
||
}
|
||
|
||
err = clBuildProgram(m_program, 1, &m_device, nullptr, nullptr, nullptr);
|
||
if (err != CL_SUCCESS) {
|
||
char buildLog[4096];
|
||
clGetProgramBuildInfo(m_program, m_device, CL_PROGRAM_BUILD_LOG, sizeof(buildLog), buildLog, nullptr);
|
||
emit errorOccurred(QString("Failed to build OpenCL program: %1").arg(buildLog));
|
||
cleanupOpenCL();
|
||
return false;
|
||
}
|
||
|
||
qDebug() << "[OpenCL] Program compiled successfully";
|
||
|
||
// 6. 创建kernel
|
||
m_kernel = clCreateKernel(m_program, "compute_xyz", &err);
|
||
if (err != CL_SUCCESS) {
|
||
emit errorOccurred("Failed to create OpenCL kernel");
|
||
cleanupOpenCL();
|
||
return false;
|
||
}
|
||
|
||
// 7. 创建缓冲区
|
||
m_depthBuffer = clCreateBuffer(m_context, CL_MEM_READ_ONLY,
|
||
sizeof(float) * m_totalPoints, nullptr, &err);
|
||
if (err != CL_SUCCESS) {
|
||
emit errorOccurred("Failed to create depth buffer");
|
||
cleanupOpenCL();
|
||
return false;
|
||
}
|
||
|
||
m_xyzBuffer = clCreateBuffer(m_context, CL_MEM_WRITE_ONLY,
|
||
sizeof(float) * m_totalPoints * 3, nullptr, &err);
|
||
if (err != CL_SUCCESS) {
|
||
emit errorOccurred("Failed to create XYZ buffer");
|
||
cleanupOpenCL();
|
||
return false;
|
||
}
|
||
|
||
m_clInitialized = true;
|
||
qDebug() << "[OpenCL] Initialization complete";
|
||
return true;
|
||
}
|
||
|
||
void PointCloudProcessor::processDepthData(const QByteArray &depthData, uint32_t blockId)
|
||
{
|
||
if (!m_clInitialized) {
|
||
if (!initializeOpenCL()) {
|
||
return;
|
||
}
|
||
}
|
||
|
||
// 验证数据大小
|
||
if (depthData.size() != m_totalPoints * sizeof(int16_t)) {
|
||
qDebug() << "[PointCloud] Invalid depth data size:" << depthData.size()
|
||
<< "expected:" << (m_totalPoints * sizeof(int16_t));
|
||
return;
|
||
}
|
||
|
||
// 转换int16_t到float
|
||
const int16_t* depthShort = reinterpret_cast<const int16_t*>(depthData.constData());
|
||
std::vector<float> depthFloat(m_totalPoints);
|
||
for (size_t i = 0; i < m_totalPoints; i++) {
|
||
depthFloat[i] = static_cast<float>(depthShort[i]);
|
||
}
|
||
|
||
// 注释掉频繁的日志输出
|
||
// qDebug() << "[PointCloud] Processing block" << blockId << "with" << m_totalPoints << "points";
|
||
|
||
// 上传深度数据到GPU
|
||
cl_int err = clEnqueueWriteBuffer(m_queue, m_depthBuffer, CL_TRUE, 0,
|
||
sizeof(float) * m_totalPoints, depthFloat.data(),
|
||
0, nullptr, nullptr);
|
||
if (err != CL_SUCCESS) {
|
||
emit errorOccurred(QString("Failed to upload depth data: %1").arg(err));
|
||
return;
|
||
}
|
||
|
||
// 设置kernel参数
|
||
float inv_fx = 1.0f / m_fx;
|
||
float inv_fy = 1.0f / m_fy;
|
||
int width = m_imageWidth;
|
||
int height = m_imageHeight;
|
||
|
||
clSetKernelArg(m_kernel, 0, sizeof(cl_mem), &m_depthBuffer);
|
||
clSetKernelArg(m_kernel, 1, sizeof(cl_mem), &m_xyzBuffer);
|
||
clSetKernelArg(m_kernel, 2, sizeof(int), &width);
|
||
clSetKernelArg(m_kernel, 3, sizeof(int), &height);
|
||
clSetKernelArg(m_kernel, 4, sizeof(float), &inv_fx);
|
||
clSetKernelArg(m_kernel, 5, sizeof(float), &inv_fy);
|
||
clSetKernelArg(m_kernel, 6, sizeof(float), &m_cx);
|
||
clSetKernelArg(m_kernel, 7, sizeof(float), &m_cy);
|
||
clSetKernelArg(m_kernel, 8, sizeof(float), &m_zScale);
|
||
|
||
// 执行kernel
|
||
size_t local = 256;
|
||
size_t global = ((m_totalPoints + local - 1) / local) * local;
|
||
err = clEnqueueNDRangeKernel(m_queue, m_kernel, 1, nullptr,
|
||
&global, &local, 0, nullptr, nullptr);
|
||
if (err != CL_SUCCESS) {
|
||
emit errorOccurred(QString("Failed to execute kernel: %1").arg(err));
|
||
return;
|
||
}
|
||
|
||
clFinish(m_queue);
|
||
|
||
// 读取结果
|
||
std::vector<float> xyz(m_totalPoints * 3);
|
||
err = clEnqueueReadBuffer(m_queue, m_xyzBuffer, CL_TRUE, 0,
|
||
sizeof(float) * m_totalPoints * 3, xyz.data(),
|
||
0, nullptr, nullptr);
|
||
if (err != CL_SUCCESS) {
|
||
emit errorOccurred(QString("Failed to read XYZ data: %1").arg(err));
|
||
return;
|
||
}
|
||
|
||
// 创建PCL点云
|
||
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
|
||
cloud->width = m_imageWidth;
|
||
cloud->height = m_imageHeight;
|
||
cloud->is_dense = false;
|
||
cloud->points.resize(m_totalPoints);
|
||
|
||
// 填充点云数据（过滤全零点）
|
||
for (size_t i = 0; i < m_totalPoints; i++) {
|
||
cloud->points[i].x = xyz[i * 3];
|
||
cloud->points[i].y = xyz[i * 3 + 1];
|
||
cloud->points[i].z = xyz[i * 3 + 2];
|
||
}
|
||
|
||
// 注释掉频繁的日志输出
|
||
// qDebug() << "[PointCloud] Block" << blockId << "processed successfully";
|
||
emit pointCloudReady(cloud, blockId);
|
||
}
|
||
|
||
void PointCloudProcessor::processPointCloudData(const QByteArray &cloudData, uint32_t blockId)
|
||
{
|
||
// qDebug() << "[PointCloud] Processing pre-computed point cloud data";
|
||
// qDebug() << "[PointCloud] Data size:" << cloudData.size() << "bytes";
|
||
|
||
// 验证数据大小：支持两种格式
|
||
// 格式1：只有Z坐标 (width * height * sizeof(int16_t))
|
||
// 格式2：完整XYZ (width * height * 3 * sizeof(int16_t))
|
||
size_t expectedSizeZ = m_imageWidth * m_imageHeight * sizeof(int16_t);
|
||
size_t expectedSizeXYZ = m_imageWidth * m_imageHeight * 3 * sizeof(int16_t);
|
||
|
||
bool isZOnly = (cloudData.size() == expectedSizeZ);
|
||
bool isXYZ = (cloudData.size() == expectedSizeXYZ);
|
||
|
||
if (!isZOnly && !isXYZ) {
|
||
qDebug() << "[PointCloud] ERROR: Invalid point cloud data size:" << cloudData.size()
|
||
<< "expected:" << expectedSizeZ << "(Z only) or" << expectedSizeXYZ << "(XYZ)";
|
||
return;
|
||
}
|
||
|
||
// qDebug() << "[PointCloud] Data format:" << (isZOnly ? "Z only" : "XYZ");
|
||
|
||
// 创建PCL点云
|
||
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
|
||
cloud->width = m_imageWidth;
|
||
cloud->height = m_imageHeight;
|
||
cloud->is_dense = false;
|
||
cloud->points.resize(m_totalPoints);
|
||
|
||
// 从int16_t数组读取点云数据
|
||
const int16_t* cloudShort = reinterpret_cast<const int16_t*>(cloudData.constData());
|
||
|
||
float inv_fx = 1.0f / m_fx;
|
||
float inv_fy = 1.0f / m_fy;
|
||
|
||
if (isZOnly) {
|
||
// Z-only格式：标准针孔模型反投影
|
||
for (size_t i = 0; i < m_totalPoints; i++) {
|
||
int row = i / m_imageWidth;
|
||
int col = i % m_imageWidth;
|
||
|
||
float z = static_cast<float>(cloudShort[i]) * m_zScale;
|
||
cloud->points[i].x = (col - m_cx) * z * inv_fx;
|
||
cloud->points[i].y = (row - m_cy) * z * inv_fy;
|
||
cloud->points[i].z = z;
|
||
}
|
||
} else {
|
||
// XYZ格式：使用Z值进行针孔模型反投影
|
||
for (size_t i = 0; i < m_totalPoints; i++) {
|
||
int row = i / m_imageWidth;
|
||
int col = i % m_imageWidth;
|
||
|
||
float z = static_cast<float>(cloudShort[i * 3 + 2]) * m_zScale;
|
||
cloud->points[i].x = (col - m_cx) * z * inv_fx;
|
||
cloud->points[i].y = (row - m_cy) * z * inv_fy;
|
||
cloud->points[i].z = z;
|
||
}
|
||
}
|
||
|
||
// qDebug() << "[PointCloud] Block" << blockId << "processed successfully,"
|
||
// << m_totalPoints << "points";
|
||
emit pointCloudReady(cloud, blockId);
|
||
}
|
||
|
||
void PointCloudProcessor::cleanupOpenCL()
|
||
{
|
||
if (m_depthBuffer) {
|
||
clReleaseMemObject(m_depthBuffer);
|
||
m_depthBuffer = nullptr;
|
||
}
|
||
if (m_xyzBuffer) {
|
||
clReleaseMemObject(m_xyzBuffer);
|
||
m_xyzBuffer = nullptr;
|
||
}
|
||
if (m_kernel) {
|
||
clReleaseKernel(m_kernel);
|
||
m_kernel = nullptr;
|
||
}
|
||
if (m_program) {
|
||
clReleaseProgram(m_program);
|
||
m_program = nullptr;
|
||
}
|
||
if (m_queue) {
|
||
clReleaseCommandQueue(m_queue);
|
||
m_queue = nullptr;
|
||
}
|
||
if (m_context) {
|
||
clReleaseContext(m_context);
|
||
m_context = nullptr;
|
||
}
|
||
m_clInitialized = false;
|
||
qDebug() << "[OpenCL] Cleanup complete";
|
||
}
|