441 lines
16 KiB
C++
441 lines
16 KiB
C++
#include "PointCloudImageUtils.h"
|
|
#include <QPainter>
|
|
#include <cmath>
|
|
#include <algorithm>
|
|
#include "SG_bagPositioning_Export.h"
|
|
#include "SG_baseDataType.h"
|
|
#include "VrLog.h"
|
|
|
|
#ifndef PI
|
|
#define PI 3.14159265358979323846
|
|
#endif
|
|
|
|
QImage PointCloudImageUtils::GeneratePointCloudImage(SVzNL3DLaserLine* scanData,
|
|
int lineNum,
|
|
const std::vector<SSG_peakRgnInfo>& objOps)
|
|
{
|
|
if (!scanData || lineNum <= 0) {
|
|
return QImage(); // 返回空图像
|
|
}
|
|
|
|
// 统计X和Y的范围
|
|
double xMin = 0.0, xMax = -1.0;
|
|
double yMin = 0.0, yMax = -1.0;
|
|
CalculatePointCloudRange(scanData, lineNum, xMin, xMax, yMin, yMax);
|
|
|
|
// 检查范围是否有效
|
|
if (xMax < xMin || yMax < yMin) {
|
|
return QImage(); // 返回空图像
|
|
}
|
|
|
|
// 创建图像
|
|
int imgRows = 992;
|
|
int imgCols = 1056;
|
|
int x_skip = 16;
|
|
int y_skip = 16;
|
|
|
|
// 计算投影比例
|
|
double y_rows = (double)(imgRows - y_skip *2);
|
|
double x_cols = (double)(imgCols - x_skip *2);
|
|
//计算投影比例
|
|
double x_scale = (xMax - xMin) / x_cols;
|
|
double y_scale = (yMax - yMin) / y_rows;
|
|
if (x_scale < y_scale)
|
|
x_scale = y_scale;
|
|
else
|
|
y_scale = x_scale;
|
|
|
|
QImage image(imgCols, imgRows, QImage::Format_RGB888);
|
|
image.fill(Qt::black);
|
|
QPainter painter(&image);
|
|
|
|
// 绘制点云
|
|
for (int line = 0; line < lineNum; line++) {
|
|
for (int i = 0; i < scanData[line].nPositionCnt; i++) {
|
|
SVzNL3DPosition* pt3D = &scanData[line].p3DPosition[i];
|
|
if (pt3D->pt3D.z < 1e-4) continue;
|
|
|
|
// 解析点索引信息
|
|
int vType = pt3D->nPointIdx & 0xff;
|
|
int hType = vType >> 4;
|
|
int objId = (pt3D->nPointIdx >> 16) & 0xff;
|
|
vType = vType & 0x0f;
|
|
|
|
// 根据线特征类型确定颜色和大小
|
|
QColor pointColor;
|
|
int pointSize = 1;
|
|
GetLineFeatureStyle(vType, hType, objId, pointColor, pointSize);
|
|
|
|
int px = (int)((pt3D->pt3D.x - xMin) / x_scale + x_skip);
|
|
int py = (int)((pt3D->pt3D.y - yMin) / y_scale + y_skip);
|
|
|
|
if (px >= 0 && px < imgCols && py >= 0 && py < imgRows) {
|
|
painter.setPen(QPen(pointColor, 1));
|
|
painter.drawPoint(px, py);
|
|
}
|
|
}
|
|
}
|
|
|
|
// 绘制检测目标和方向线
|
|
DrawDetectionTargets(painter, objOps, xMin, x_scale, x_skip, yMin, y_scale, y_skip, imgCols, imgRows);
|
|
|
|
return image;
|
|
}
|
|
|
|
QImage PointCloudImageUtils::GeneratePointCloudImage(SVzNLXYZRGBDLaserLine* scanData,
|
|
int lineNum,
|
|
const std::vector<SSG_peakOrienRgnInfo>& objOps)
|
|
{
|
|
|
|
int imgRows = 992;
|
|
int imgCols = 1056;
|
|
QImage image(imgCols, imgRows, QImage::Format_RGB888);
|
|
image.fill(Qt::black);
|
|
|
|
|
|
if (!scanData || lineNum <= 0) {
|
|
return image; // 返回空图像
|
|
}
|
|
|
|
// 统计X和Y的范围 - 参考_RGBDto2DImage函数
|
|
double xMin = 0.0, xMax = -1.0;
|
|
double yMin = 0.0, yMax = -1.0;
|
|
|
|
for (int line = 0; line < lineNum; line++) {
|
|
for (int i = 0; i < scanData[line].nPointCnt; i++) {
|
|
SVzNLPointXYZRGBA* pt3D = &scanData[line].p3DPoint[i];
|
|
if (pt3D->z < 1e-4) continue;
|
|
|
|
// 更新X范围
|
|
if (xMax < xMin) {
|
|
xMin = xMax = pt3D->x;
|
|
} else {
|
|
if (xMin > pt3D->x) xMin = pt3D->x;
|
|
if (xMax < pt3D->x) xMax = pt3D->x;
|
|
}
|
|
|
|
// 更新Y范围
|
|
if (yMax < yMin) {
|
|
yMin = yMax = pt3D->y;
|
|
} else {
|
|
if (yMin > pt3D->y) yMin = pt3D->y;
|
|
if (yMax < pt3D->y) yMax = pt3D->y;
|
|
}
|
|
}
|
|
}
|
|
|
|
// 检查范围是否有效
|
|
if (xMax < xMin || yMax < yMin) {
|
|
return image; // 返回空图像
|
|
}
|
|
|
|
// 创建图像 - 参考_RGBDto2DImage函数的尺寸和偏移
|
|
int x_skip = 16;
|
|
int y_skip = 16;
|
|
double y_rows = (double)(imgRows - y_skip * 2);
|
|
double x_cols = (double)(imgCols - x_skip * 2);
|
|
|
|
// 计算投影比例 - 参考_RGBDto2DImage函数的比例计算
|
|
double x_scale = (xMax - xMin) / x_cols;
|
|
double y_scale = (yMax - yMin) / y_rows;
|
|
if (x_scale < y_scale)
|
|
x_scale = y_scale;
|
|
else
|
|
y_scale = x_scale;
|
|
|
|
QPainter painter(&image);
|
|
|
|
// 绘制点云 - 参考_RGBDto2DImage函数的绘制方式
|
|
for (int line = 0; line < lineNum; line++) {
|
|
for (int i = 0; i < scanData[line].nPointCnt; i++) {
|
|
SVzNLPointXYZRGBA* pt3D = &scanData[line].p3DPoint[i];
|
|
if (pt3D->z < 1e-4) continue;
|
|
|
|
// 解析RGB颜色 - 参考_RGBDto2DImage函数的解析方式
|
|
int nRGB = pt3D->nRGB;
|
|
int r = nRGB & 0xff;
|
|
nRGB >>= 8;
|
|
int g = nRGB & 0xff;
|
|
nRGB >>= 8;
|
|
int b = nRGB & 0xff;
|
|
|
|
QColor pointColor(r, g, b);
|
|
|
|
int px = (int)((pt3D->x - xMin) / x_scale + x_skip);
|
|
int py = (int)((pt3D->y - yMin) / y_scale + y_skip);
|
|
|
|
if (px >= 0 && px < imgCols && py >= 0 && py < imgRows) {
|
|
painter.setPen(QPen(pointColor, 1));
|
|
painter.drawPoint(px, py);
|
|
}
|
|
}
|
|
}
|
|
|
|
// 绘制检测目标和方向线
|
|
DrawDetectionTargets(painter, objOps, xMin, x_scale, x_skip, yMin, y_scale, y_skip, imgCols, imgRows);
|
|
|
|
return image;
|
|
}
|
|
|
|
void PointCloudImageUtils::GetLineFeatureStyle(int vType, int hType, int objId,
|
|
QColor& pointColor, int& pointSize)
|
|
{
|
|
pointSize = 1;
|
|
|
|
// 优先根据垂直方向特征设置颜色
|
|
if (LINE_FEATURE_L_JUMP_H2L == vType) {
|
|
pointColor = QColor(255, 97, 0); // 橙色
|
|
pointSize = 2;
|
|
}
|
|
else if (LINE_FEATURE_L_JUMP_L2H == vType) {
|
|
pointColor = QColor(255, 255, 0); // 黄色
|
|
pointSize = 2;
|
|
}
|
|
else if (LINE_FEATURE_V_SLOPE == vType) {
|
|
pointColor = QColor(255, 0, 255); // 紫色
|
|
pointSize = 2;
|
|
}
|
|
else if (LINE_FEATURE_L_SLOPE_H2L == vType) {
|
|
pointColor = QColor(160, 82, 45); // 褐色
|
|
pointSize = 2;
|
|
}
|
|
else if ((LINE_FEATURE_LINE_ENDING_0 == vType) || (LINE_FEATURE_LINE_ENDING_1 == vType)) {
|
|
pointColor = QColor(255, 0, 0); // 红色
|
|
pointSize = 2;
|
|
}
|
|
else if (LINE_FEATURE_L_SLOPE_L2H == vType) {
|
|
pointColor = QColor(233, 150, 122); // 浅褐色
|
|
pointSize = 2;
|
|
}
|
|
// 检查水平方向特征
|
|
else if (LINE_FEATURE_L_JUMP_H2L == hType) {
|
|
pointColor = QColor(0, 0, 255); // 蓝色
|
|
pointSize = 2;
|
|
}
|
|
else if (LINE_FEATURE_L_JUMP_L2H == hType) {
|
|
pointColor = QColor(0, 255, 255); // 青色
|
|
pointSize = 2;
|
|
}
|
|
else if (LINE_FEATURE_V_SLOPE == hType) {
|
|
pointColor = QColor(0, 255, 0); // 绿色
|
|
pointSize = 2;
|
|
}
|
|
else if (LINE_FEATURE_L_SLOPE_H2L == hType) {
|
|
pointColor = QColor(85, 107, 47); // 橄榄绿
|
|
pointSize = 2;
|
|
}
|
|
else if (LINE_FEATURE_L_SLOPE_L2H == hType) {
|
|
pointColor = QColor(0, 255, 154); // 浅绿色
|
|
pointSize = 2;
|
|
}
|
|
else if ((LINE_FEATURE_LINE_ENDING_0 == hType) || (LINE_FEATURE_LINE_ENDING_1 == hType)) {
|
|
pointColor = QColor(255, 0, 0); // 红色
|
|
pointSize = 3;
|
|
}
|
|
// 检查是否为目标对象
|
|
else if (objId > 0) {
|
|
pointColor = GetObjectColor(objId);
|
|
pointSize = 1;
|
|
}
|
|
// 默认颜色
|
|
else {
|
|
pointColor = QColor(150, 150, 150); // 深灰色
|
|
pointSize = 1;
|
|
}
|
|
}
|
|
|
|
QColor PointCloudImageUtils::GetObjectColor(int index)
|
|
{
|
|
QColor objColors[8] = {
|
|
QColor(245,222,179), QColor(210,105,30), QColor(240,230,140), QColor(135,206,235),
|
|
QColor(250,235,215), QColor(189,252,201), QColor(221,160,221), QColor(188,143,143)
|
|
};
|
|
|
|
return objColors[index % 8];
|
|
}
|
|
|
|
void PointCloudImageUtils::CalculatePointCloudRange(SVzNL3DLaserLine* scanData, int lineNum,
|
|
double& xMin, double& xMax,
|
|
double& yMin, double& yMax)
|
|
{
|
|
xMin = 0.0; xMax = -1.0;
|
|
yMin = 0.0; yMax = -1.0;
|
|
|
|
for (int line = 0; line < lineNum; line++) {
|
|
for (int i = 0; i < scanData[line].nPositionCnt; i++) {
|
|
SVzNL3DPosition* pt3D = &scanData[line].p3DPosition[i];
|
|
if (pt3D->pt3D.z < 1e-4) continue;
|
|
|
|
// 更新X范围
|
|
if (xMax < xMin) {
|
|
xMin = xMax = pt3D->pt3D.x;
|
|
} else {
|
|
if (xMin > pt3D->pt3D.x) xMin = pt3D->pt3D.x;
|
|
if (xMax < pt3D->pt3D.x) xMax = pt3D->pt3D.x;
|
|
}
|
|
|
|
// 更新Y范围
|
|
if (yMax < yMin) {
|
|
yMin = yMax = pt3D->pt3D.y;
|
|
} else {
|
|
if (yMin > pt3D->pt3D.y) yMin = pt3D->pt3D.y;
|
|
if (yMax < pt3D->pt3D.y) yMax = pt3D->pt3D.y;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void PointCloudImageUtils::DrawDetectionTargets(QPainter& painter,
|
|
const std::vector<SSG_peakRgnInfo>& objOps,
|
|
double xMin, double xScale, int xSkip,
|
|
double yMin, double yScale, int ySkip,
|
|
int imgCols, int imgRows)
|
|
{
|
|
// 绘制检测目标和方向线
|
|
for (size_t i = 0; i < objOps.size(); i++) {
|
|
QColor objColor = (i == 0) ? QColor(255, 0, 0) : QColor(255, 255, 0);
|
|
int size = (i == 0) ? 12 : 8;
|
|
|
|
int px = (int)((objOps[i].centerPos.x - xMin) / xScale + xSkip);
|
|
int py = (int)((objOps[i].centerPos.y - yMin) / yScale + ySkip);
|
|
|
|
if (px >= 0 && px < imgCols && py >= 0 && py < imgRows) {
|
|
// 绘制抓取点圆圈
|
|
painter.setPen(QPen(objColor, 2));
|
|
painter.setBrush(QBrush(objColor));
|
|
painter.drawEllipse(px - size/2, py - size/2, size, size);
|
|
|
|
// 绘制方向线
|
|
const double deg2rad = PI / 180.0;
|
|
|
|
// 使用检测目标实际2D尺寸的较大值的一半作为方向线长度
|
|
double maxSize = std::max(objOps[i].objSize.dHeight, objOps[i].objSize.dWidth);
|
|
double R = std::max(20.0, maxSize / 6.0);
|
|
|
|
const double yaw = objOps[i].centerPos.z_yaw * deg2rad;
|
|
double cy = cos(yaw);
|
|
double sy = sin(yaw);
|
|
double x1 = objOps[i].centerPos.x + R * cy;
|
|
double y1 = objOps[i].centerPos.y - R * sy;
|
|
double x2 = objOps[i].centerPos.x - R * cy;
|
|
double y2 = objOps[i].centerPos.y + R * sy;
|
|
|
|
int px1 = (int)((x1 - xMin) / xScale + xSkip);
|
|
int py1 = (int)((y1 - yMin) / yScale + ySkip);
|
|
int px2 = (int)((x2 - xMin) / xScale + xSkip);
|
|
int py2 = (int)((y2 - yMin) / yScale + ySkip);
|
|
|
|
// 绘制方向线
|
|
painter.setPen(QPen(objColor, 3));
|
|
painter.drawLine(px1, py1, px2, py2);
|
|
|
|
// 绘制目标编号
|
|
painter.setPen(QPen(Qt::white, 1));
|
|
painter.setFont(QFont("Arial", 15, QFont::Bold));
|
|
painter.drawText(px + 15, py - 10, QString("%1").arg(i + 1));
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
void PointCloudImageUtils::DrawDetectionTargets(QPainter& painter,
|
|
const std::vector<SSG_peakOrienRgnInfo>& objOps,
|
|
double xMin, double xScale, int xSkip,
|
|
double yMin, double yScale, int ySkip,
|
|
int imgCols, int imgRows)
|
|
{
|
|
// 绘制检测目标和方向线 - 参考bagPositioning_test.cpp中的_XOYprojection_RGBD函数
|
|
for (size_t i = 0; i < objOps.size(); i++) {
|
|
QColor objColor = (i == 0) ? QColor(255, 0, 0) : QColor(255, 255, 0);
|
|
int size = (i == 0) ? 20 : 10;
|
|
|
|
int px = (int)((objOps[i].centerPos.x - xMin) / xScale + xSkip);
|
|
int py = (int)((objOps[i].centerPos.y - yMin) / yScale + ySkip);
|
|
|
|
if (px >= 0 && px < imgCols && py >= 0 && py < imgRows) {
|
|
// 绘制抓取点圆圈
|
|
painter.setPen(QPen(objColor, 2));
|
|
painter.setBrush(QBrush(objColor));
|
|
painter.drawEllipse(px - size/2, py - size/2, size, size);
|
|
|
|
// 绘制方向线 - 参考bagPositioning_test.cpp中的实现
|
|
const double deg2rad = PI / 180.0;
|
|
|
|
// 使用检测目标实际2D尺寸的较大值的一半作为方向线长度
|
|
double maxSize = std::max(objOps[i].objSize.dHeight, objOps[i].objSize.dWidth);
|
|
double R = std::max(20.0, maxSize / 6.0);
|
|
|
|
const double yaw = objOps[i].centerPos.z_yaw * deg2rad;
|
|
double cy = cos(yaw);
|
|
double sy = sin(yaw);
|
|
|
|
// 计算方向线的端点
|
|
double x1 = objOps[i].centerPos.x + R * cy;
|
|
double y1 = objOps[i].centerPos.y - R * sy;
|
|
double x2 = objOps[i].centerPos.x - R * cy;
|
|
double y2 = objOps[i].centerPos.y + R * sy;
|
|
|
|
int px1 = (int)((x1 - xMin) / xScale + xSkip);
|
|
int py1 = (int)((y1 - yMin) / yScale + ySkip);
|
|
int px2 = (int)((x2 - xMin) / xScale + xSkip);
|
|
int py2 = (int)((y2 - yMin) / yScale + ySkip);
|
|
|
|
// 绘制方向线
|
|
painter.setPen(QPen(objColor, 2));
|
|
painter.drawLine(px1, py1, px2, py2);
|
|
|
|
// 根据orienFlag绘制箭头
|
|
if (objOps[i].orienFlag == 1) {
|
|
// 绿色箭头 - 正面
|
|
painter.setPen(QPen(QColor(0, 255, 0), 2));
|
|
|
|
// 计算箭头端点
|
|
double arrowLen = R / 3;
|
|
double arrowAngle = 30 * deg2rad;
|
|
double ca = cos(arrowAngle);
|
|
double sa = sin(arrowAngle);
|
|
|
|
double x3 = x1 - arrowLen * ca * cy + arrowLen * sa * sy;
|
|
double y3 = y1 + arrowLen * ca * sy + arrowLen * sa * cy;
|
|
double x4 = x1 - arrowLen * ca * cy - arrowLen * sa * sy;
|
|
double y4 = y1 + arrowLen * ca * sy - arrowLen * sa * cy;
|
|
|
|
int px3 = (int)((x3 - xMin) / xScale + xSkip);
|
|
int py3 = (int)((y3 - yMin) / yScale + ySkip);
|
|
int px4 = (int)((x4 - xMin) / xScale + xSkip);
|
|
int py4 = (int)((y4 - yMin) / yScale + ySkip);
|
|
|
|
painter.drawLine(px1, py1, px3, py3);
|
|
painter.drawLine(px1, py1, px4, py4);
|
|
}
|
|
else if (objOps[i].orienFlag == 2) {
|
|
// 蓝色箭头 - 反面
|
|
painter.setPen(QPen(QColor(0, 0, 255), 2));
|
|
|
|
// 计算箭头端点
|
|
double arrowLen = R / 3;
|
|
double arrowAngle = 30 * deg2rad;
|
|
double ca = cos(arrowAngle);
|
|
double sa = sin(arrowAngle);
|
|
|
|
double x3 = x1 - arrowLen * ca * cy + arrowLen * sa * sy;
|
|
double y3 = y1 + arrowLen * ca * sy + arrowLen * sa * cy;
|
|
double x4 = x1 - arrowLen * ca * cy - arrowLen * sa * sy;
|
|
double y4 = y1 + arrowLen * ca * sy - arrowLen * sa * cy;
|
|
|
|
int px3 = (int)((x3 - xMin) / xScale + xSkip);
|
|
int py3 = (int)((y3 - yMin) / yScale + ySkip);
|
|
int px4 = (int)((x4 - xMin) / xScale + xSkip);
|
|
int py4 = (int)((y4 - yMin) / yScale + ySkip);
|
|
|
|
painter.drawLine(px1, py1, px3, py3);
|
|
painter.drawLine(px1, py1, px4, py4);
|
|
}
|
|
|
|
// 绘制目标编号
|
|
painter.setPen(QPen(Qt::white, 1));
|
|
painter.setFont(QFont("Arial", 15, QFont::Bold));
|
|
painter.drawText(px + 15, py - 10, QString("%1").arg(i + 1));
|
|
}
|
|
}
|
|
}
|