From 71e8f124803d9741c53af204365d025e7ac89596 Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Tue, 15 Jul 2025 21:35:15 +0800 Subject: [PATCH 1/4] =?UTF-8?q?QRCode=EF=BC=88DM=20code)=20algorithm=20and?= =?UTF-8?q?=20test=20projects=20initial=20check=20in?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- QRcode3Ddetection/QRcode3Ddetection.vcxproj | 172 ++++ .../QRcode3Ddetection_test.cpp | 745 ++++++++++++++++++ .../QRcode3Ddetection_test.vcxproj | 157 ++++ 3 files changed, 1074 insertions(+) create mode 100644 QRcode3Ddetection/QRcode3Ddetection.vcxproj create mode 100644 QRcode3Ddetection_test/QRcode3Ddetection_test.cpp create mode 100644 QRcode3Ddetection_test/QRcode3Ddetection_test.vcxproj diff --git a/QRcode3Ddetection/QRcode3Ddetection.vcxproj b/QRcode3Ddetection/QRcode3Ddetection.vcxproj new file mode 100644 index 0000000..a096ca7 --- /dev/null +++ b/QRcode3Ddetection/QRcode3Ddetection.vcxproj @@ -0,0 +1,172 @@ + + + + + Debug + Win32 + + + Release + Win32 + + + Debug + x64 + + + Release + x64 + + + + + + + + + + 16.0 + Win32Proj + {a7c72ed9-b81d-4c6a-be38-d75199c824aa} + QRcode3Ddetection + 10.0 + + + + DynamicLibrary + true + v142 + Unicode + + + DynamicLibrary + false + v142 + true + Unicode + + + DynamicLibrary + true + v142 + Unicode + + + DynamicLibrary + false + v142 + true + Unicode + + + + + + + + + + + + + + + + + + + + + true + + + false + + + true + $(SolutionDir)build\$(Platform)\$(Configuration)\ + ..\..\thirdParty\VzNLSDK\Inc;..\sourceCode;..\sourceCode\inc;$(IncludePath) + + + false + $(SolutionDir)build\$(Platform)\$(Configuration)\ + ..\..\thirdParty\VzNLSDK\Inc;..\..\thirdParty\opencv\build\include;..\sourceCode;..\sourceCode\inc;$(IncludePath) + + + + Level3 + true + WIN32;_DEBUG;QRCODE3DDETECTION_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) + true + Use + pch.h + + + Windows + true + false + + + + + Level3 + true + true + true + WIN32;NDEBUG;QRCODE3DDETECTION_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) + true + Use + pch.h + + + Windows + true + true + true + false + + + + + Level3 + true + SG_API_LIBRARY;_DEBUG;QRCODE3DDETECTION_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) + true + NotUsing + pch.h + ..\..\thirdParty\opencv\build\include;%(AdditionalIncludeDirectories) + + + Windows + true + false + ..\..\thirdParty\opencv\build\x64\vc16\lib;..\build\x64\Debug;%(AdditionalLibraryDirectories) + opencv_world480d.lib;baseAlgorithm.lib;%(AdditionalDependencies) + + + + + Level3 + true + true + true + SG_API_LIBRARY;NDEBUG;QRCODE3DDETECTION_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) + true + NotUsing + pch.h + ..\..\thirdParty\opencv\build\include;%(AdditionalIncludeDirectories) + + + Windows + true + true + true + false + ..\..\thirdParty\opencv\build\x64\vc16\lib;..\build\x64\Release;%(AdditionalLibraryDirectories) + opencv_world480.lib;baseAlgorithm.lib;%(AdditionalDependencies) + + + + + + \ No newline at end of file diff --git a/QRcode3Ddetection_test/QRcode3Ddetection_test.cpp b/QRcode3Ddetection_test/QRcode3Ddetection_test.cpp new file mode 100644 index 0000000..cd8b884 --- /dev/null +++ b/QRcode3Ddetection_test/QRcode3Ddetection_test.cpp @@ -0,0 +1,745 @@ +// QRcode3Ddetection_test.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。 +// + +#include +#include +#include +#include +#include +#include "direct.h" +#include +#include "WD_QRcode3Ddetection_Export.h" +#include +#include + +typedef struct +{ + int r; + int g; + int b; +}SG_color; + +void vzReadLaserScanPointFromFile_plyTxt(const char* fileName, std::vector< SVzNL3DPoint>& scanData, bool removeZeros, bool exchangeXY) +{ + std::ifstream inputFile(fileName); + std::string linedata; + + if (inputFile.is_open() == false) + return; + + while (std::getline(inputFile, linedata)) + { + if (linedata.empty()) + continue; + + double X, Y, Z, tmp; + sscanf_s(linedata.c_str(), "%lf %lf %lf %lf", &X, &Y, &Z, &tmp); + if (true == removeZeros) + { + if (Z > 1e-4) + { + SVzNL3DPoint a_pt; + if (true == exchangeXY) + { + a_pt.x = Y; //将扫描线调整为Y方向；扫描方向为X方向 + a_pt.y = X; + } + else + { + a_pt.x = X; + a_pt.y = Y; + } + a_pt.z = Z; + scanData.push_back(a_pt); + } + } + else + { + SVzNL3DPoint a_pt; + if (true == exchangeXY) + { + a_pt.x = Y; //将扫描线调整为Y方向；扫描方向为X方向 + a_pt.y = X; + } + else + { + a_pt.x = X; + a_pt.y = Y; + } + a_pt.z = Z; + scanData.push_back(a_pt); + } + } + return; +} + +void _outputScanDataFile_vector(char* fileName, std::vector>& scanLines, bool removeZeros, int* headNullLines) +{ + std::ofstream sw(fileName); + int lineNum = scanLines.size(); + if (lineNum == 0) + return; + + sw << "LineNum:" << lineNum << std::endl; + sw << "DataType: 0" << std::endl; + sw << "ScanSpeed: 0" << std::endl; + sw << "PointAdjust: 1" << std::endl; + sw << "MaxTimeStamp: 0_0" << std::endl; + + int lineIdx = 0; + int null_lines = 0; + bool counterNull = true; + for (int line = 0; line < lineNum; line++) + { + int linePtNum = scanLines[line].size(); + if (linePtNum == 0) + continue; + + if (true == removeZeros) + { + int vldPtNum = 0; + for (int i = 0; i < linePtNum; i++) + { + if (scanLines[line][i].pt3D.z > 1e-4) + vldPtNum++; + } + linePtNum = vldPtNum; + } + sw << "Line_" << lineIdx << "_0_" << linePtNum << std::endl; + lineIdx++; + bool isNull = true; + for (int i = 0; i < linePtNum; i++) + { + SVzNL3DPoint* pt3D = &scanLines[line][i].pt3D; + if ((pt3D->z > 1e-4) && (isNull == true)) + isNull = false; + if ((true == removeZeros) && (pt3D->z < 1e-4)) + continue; + float x = (float)pt3D->x; + float y = (float)pt3D->y; + float z = (float)pt3D->z; + sw << "{ " << x << "," << y << "," << z << " }-"; + sw << "{0,0}-{0,0}" << std::endl; + } + if (true == counterNull) + { + if (true == isNull) + null_lines++; + else + counterNull = false; + } + } + *headNullLines = null_lines; + sw.close(); +} + +void _outputRGBDScanDataFile_RGBD( + char* fileName, + std::vector>& scanLines, + std::vector< SVzNL3DPosition> objPoints) +{ + int lineNum = scanLines.size(); + std::ofstream sw(fileName); + int realLines = lineNum; + if (objPoints.size() > 0) + realLines++; + + sw << "LineNum:" << realLines << std::endl; + sw << "DataType: 0" << std::endl; + sw << "ScanSpeed: 0" << std::endl; + sw << "PointAdjust: 1" << std::endl; + sw << "MaxTimeStamp: 0_0" << std::endl; + + int maxLineIndex = 0; + int max_stamp = 0; + + SG_color rgb = { 0, 0, 0 }; + + SG_color objColor[8] = { + {245,222,179},//淡黄色 + {210,105, 30},//巧克力色 + {240,230,140},//黄褐色 + {135,206,235},//天蓝色 + {250,235,215},//古董白 + {189,252,201},//薄荷色 + {221,160,221},//梅红色 + {188,143,143},//玫瑰红色 + }; + int size = 1; + int lineIdx = 0; + for (int line = 0; line < lineNum; line++) + { + int linePtNum = scanLines[line].size(); + if (linePtNum == 0) + continue; + + sw << "Line_" << lineIdx << "_0_" << linePtNum << std::endl; + lineIdx++; + for (int i = 0; i < linePtNum; i++) + { + SVzNL3DPosition* pt3D = &scanLines[line][i]; + int featureType = pt3D->nPointIdx; + if (LINE_FEATURE_PEAK_TOP == featureType) + { + rgb = { 255, 97, 0 }; + size = 5; + } + else + { + rgb = { 200, 200, 200 }; + size = 1; + } + float x = (float)pt3D->pt3D.x; + float y = (float)pt3D->pt3D.y; + float z = (float)pt3D->pt3D.z; + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; + } + } + if (objPoints.size() > 0) + { + int linePtNum = objPoints.size(); + sw << "Line_" << lineNum << "_0_" << linePtNum+1 << std::endl; + + rgb = { 0, 0, 255 }; + size = 25; + for (int i = 0; i < linePtNum; i++) + { + float x = (float)objPoints[i].pt3D.x; + float y = (float)objPoints[i].pt3D.y; + float z = (float)objPoints[i].pt3D.z; + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; + } + float x = (float)objPoints[0].pt3D.x; + float y = (float)objPoints[0].pt3D.y; + float z = (float)objPoints[0].pt3D.z; + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; + } + sw.close(); +} + + +void _outputScanDataFile_ptr(char* fileName, SVzNL3DLaserLine* scanData, int lineNum) +{ + std::ofstream sw(fileName); + sw << "LineNum:" << lineNum << std::endl; + sw << "DataType: 0" << std::endl; + sw << "ScanSpeed: 0" << std::endl; + sw << "PointAdjust: 1" << std::endl; + sw << "MaxTimeStamp: 0_0" << std::endl; + + for (int line = 0; line < lineNum; line++) + { + sw << "Line_" << line << "_" << scanData[line].nTimeStamp << "_" << scanData[line].nPositionCnt << std::endl; + for (int i = 0; i < scanData[line].nPositionCnt; i++) + { + SVzNL3DPosition* pt3D = &scanData[line].p3DPosition[i]; + float x = (float)pt3D->pt3D.x; + float y = (float)pt3D->pt3D.y; + float z = (float)pt3D->pt3D.z; + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}" << std::endl; + } + } + sw.close(); +} + + +#define DATA_VER_OLD 0 +#define DATA_VER_NEW 1 +#define DATA_VER_FROM_CUSTOM 2 +#define VZ_LASER_LINE_PT_MAX_NUM 4096 +SVzNL3DLaserLine* vzReadLaserScanPointFromFile_XYZ(const char* fileName, int* scanLineNum, float* scanV, + int* dataCalib, int* scanMaxStamp, int* canClockUnit) +{ + std::ifstream inputFile(fileName); + std::string linedata; + + if (inputFile.is_open() == false) + return NULL; + + SVzNL3DLaserLine* _scanLines = NULL; + + int lines = 0; + int dataElements = 4; + int firstIndex = -1; + + int dataFileVer = DATA_VER_OLD; + std::getline(inputFile, linedata); //第一行 + int lineNum = 0; + if (0 == strncmp("LineNum:", linedata.c_str(), 8)) + { + dataFileVer = DATA_VER_NEW; + sscanf_s(linedata.c_str(), "LineNum:%d", &lines); + if (lines == 0) + return NULL; + lineNum = lines; + _scanLines = (SVzNL3DLaserLine*)malloc(sizeof(SVzNL3DLaserLine) * (lineNum + 1)); + memset(_scanLines, 0, sizeof(SVzNL3DLaserLine) * (lineNum + 1)); + if (scanLineNum) + *scanLineNum = lines; + } + else if (0 == strncmp("LineNum_", linedata.c_str(), 8)) + { + dataFileVer = DATA_VER_OLD; + sscanf_s(linedata.c_str(), "LineNum_%d", &lines); + if (lines == 0) + return NULL; + lineNum = lines; + _scanLines = (SVzNL3DLaserLine*)malloc(sizeof(SVzNL3DLaserLine) * (lineNum + 1)); + memset(_scanLines, 0, sizeof(SVzNL3DLaserLine) * (lineNum + 1)); + if (scanLineNum) + *scanLineNum = lines; + } + if (_scanLines == NULL) + return NULL; + + int ptNum = 0; + int lineIdx = -1; + int ptIdx = 0; + SVzNL3DPosition* p3DPoint = NULL; + if (dataFileVer == DATA_VER_NEW) + { + while (getline(inputFile, linedata)) + { + if (0 == strncmp("ScanSpeed:", linedata.c_str(), 10)) + { + double lineV = 0; + sscanf_s(linedata.c_str(), "ScanSpeed:%lf", &lineV); + if (scanV) + *scanV = (float)lineV; + } + else if (0 == strncmp("PointAdjust:", linedata.c_str(), 12)) + { + int ptAdjusted = 0; + sscanf_s(linedata.c_str(), "PointAdjust:%d", &ptAdjusted); + if (dataCalib) + *dataCalib = ptAdjusted; + } + else if (0 == strncmp("MaxTimeStamp:", linedata.c_str(), 13)) + { + unsigned int maxTimeStamp = 0; + unsigned int timePerStamp = 0; + sscanf_s(linedata.c_str(), "MaxTimeStamp:%u_%u", &maxTimeStamp, &timePerStamp); + if (scanMaxStamp) + *scanMaxStamp = maxTimeStamp; + if (canClockUnit) + *canClockUnit = timePerStamp; + } + else if (0 == strncmp("Line_", linedata.c_str(), 5)) + { + int lineIndex; + unsigned int timeStamp; + sscanf_s(linedata.c_str(), "Line_%d_%u_%d", &lineIndex, &timeStamp, &ptNum); + if (firstIndex < 0) + firstIndex = lineIndex; + + lineIndex = lineIndex - firstIndex; + if ((lineIndex < 0) || (lineIndex >= lines)) + break; + + //new Line + lineIdx++; + if (ptNum > 0) + { + p3DPoint = (SVzNL3DPosition*)malloc(sizeof(SVzNL3DPosition) * ptNum); + memset(p3DPoint, 0, sizeof(SVzNL3DPosition) * ptNum); + } + else + p3DPoint = NULL; + _scanLines[lineIdx].nPositionCnt = 0; + _scanLines[lineIdx].nTimeStamp = timeStamp; + _scanLines[lineIdx].p3DPosition = p3DPoint; + + } + else if (0 == strncmp("{", linedata.c_str(), 1)) + { + float X, Y, Z; + int imageY = 0; + float leftX, leftY; + float rightX, rightY; + sscanf_s(linedata.c_str(), "{%f,%f,%f}-{%f,%f}-{%f,%f}", &X, &Y, &Z, &leftX, &leftY, &rightX, &rightY); + int id = _scanLines[lineIdx].nPositionCnt; + if (id < ptNum) + { + p3DPoint[id].pt3D.x = X; + p3DPoint[id].pt3D.y = Y; + p3DPoint[id].pt3D.z = Z; + _scanLines[lineIdx].nPositionCnt = id + 1; + } + } + } + + } + else if (dataFileVer == DATA_VER_OLD) + { + while (getline(inputFile, linedata)) + { + if (0 == strncmp("DataElements_", linedata.c_str(), 13)) + { + sscanf_s(linedata.c_str(), "DataElements_%d", &dataElements); + if ((dataElements != 3) && (dataElements != 4)) + break; + } + if (0 == strncmp("LineV_", linedata.c_str(), 6)) + { + double lineV = 0; + sscanf_s(linedata.c_str(), "LineV_%lf", &lineV); + } + else if (0 == strncmp("Line_", linedata.c_str(), 5)) + { + int lineIndex; + unsigned int timeStamp; + sscanf_s(linedata.c_str(), "Line_%d_%u", &lineIndex, &timeStamp); +#if 0 + if (scanLineListTail == NULL) + firstIndex = lineIndex; +#endif + lineIndex = lineIndex - firstIndex; + if ((lineIndex < 0) || (lineIndex >= lines)) + break; + //new Line + //new Line + lineIdx++; + p3DPoint = (SVzNL3DPosition*)malloc(sizeof(SVzNL3DPosition) * VZ_LASER_LINE_PT_MAX_NUM); + memset(p3DPoint, 0, sizeof(SVzNL3DPosition) * VZ_LASER_LINE_PT_MAX_NUM); + _scanLines[lineIdx].nPositionCnt = 0; + _scanLines[lineIdx].nTimeStamp = timeStamp; + _scanLines[lineIdx].p3DPosition = p3DPoint; + } + else if (0 == strncmp("(", linedata.c_str(), 1)) + { + float X, Y, Z; + int imageY = 0; + if (dataElements == 4) + sscanf_s(linedata.c_str(), "(%f,%f,%f,%d)", &X, &Y, &Z, &imageY); + else + sscanf_s(linedata.c_str(), "(%f,%f,%f)", &X, &Y, &Z); + int id = _scanLines[lineIdx].nPositionCnt; + if (id < VZ_LASER_LINE_PT_MAX_NUM) + { + p3DPoint[id].pt3D.x = X; + p3DPoint[id].pt3D.y = Y; + p3DPoint[id].pt3D.z = Z; + _scanLines[lineIdx].nPositionCnt = id + 1; + } + } + } + } + inputFile.close(); + return _scanLines; +} + +void vzReadLaserScanPointFromFile_XYZ_vector(const char* fileName, std::vector< SVzNL3DPoint>& scanData) +{ + std::ifstream inputFile(fileName); + std::string linedata; + + if (inputFile.is_open() == false) + return; + + while (getline(inputFile, linedata)) + { + if (0 == strncmp("{", linedata.c_str(), 1)) + { + float X, Y, Z; + int imageY = 0; + float leftX, leftY; + float rightX, rightY; + sscanf_s(linedata.c_str(), "{%f,%f,%f}-{%f,%f}-{%f,%f}", &X, &Y, &Z, &leftX, &leftY, &rightX, &rightY); + SVzNL3DPoint a_pt; + a_pt.x = X; + a_pt.y = Y; + a_pt.z = Z; + scanData.push_back(a_pt); + } + } + inputFile.close(); + return; +} + +void _outputCalibPara(char* fileName, SSG_planeCalibPara calibPara) +{ + std::ofstream sw(fileName); + char dataStr[250]; + //调平矩阵 + sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.planeCalib[0], calibPara.planeCalib[1], calibPara.planeCalib[2]); + sw << dataStr << std::endl; + sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.planeCalib[3], calibPara.planeCalib[4], calibPara.planeCalib[5]); + sw << dataStr << std::endl; + sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.planeCalib[6], calibPara.planeCalib[7], calibPara.planeCalib[8]); + sw << dataStr << std::endl; + //地面高度 + sprintf_s(dataStr, 250, "%g", calibPara.planeHeight); + sw << dataStr << std::endl; + //反向旋转矩阵 + sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.invRMatrix[0], calibPara.invRMatrix[1], calibPara.invRMatrix[2]); + sw << dataStr << std::endl; + sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.invRMatrix[3], calibPara.invRMatrix[4], calibPara.invRMatrix[5]); + sw << dataStr << std::endl; + sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.invRMatrix[6], calibPara.invRMatrix[7], calibPara.invRMatrix[8]); + sw << dataStr << std::endl; + + sw.close(); +} + +SSG_planeCalibPara _readCalibPara(char* fileName) +{ + //设置初始结果 + double initCalib[9] = { + 1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0 }; + SSG_planeCalibPara planePara; + for (int i = 0; i < 9; i++) + planePara.planeCalib[i] = initCalib[i]; + planePara.planeHeight = -1.0; + for (int i = 0; i < 9; i++) + planePara.invRMatrix[i] = initCalib[i]; + + std::ifstream inputFile(fileName); + std::string linedata; + + if (inputFile.is_open() == false) + return planePara; + + //调平矩阵 + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.planeCalib[0], &planePara.planeCalib[1], &planePara.planeCalib[2]); + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.planeCalib[3], &planePara.planeCalib[4], &planePara.planeCalib[5]); + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.planeCalib[6], &planePara.planeCalib[7], &planePara.planeCalib[8]); + //地面高度 + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), "%lf", &planePara.planeHeight); + //反向旋转矩阵 + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.invRMatrix[0], &planePara.invRMatrix[1], &planePara.invRMatrix[2]); + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.invRMatrix[3], &planePara.invRMatrix[4], &planePara.invRMatrix[5]); + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.invRMatrix[6], &planePara.invRMatrix[7], &planePara.invRMatrix[8]); + + inputFile.close(); + return planePara; +} + +void _getRoiClouds( + std::vector< std::vector>& scanLines, + int startLine, + int endLine, + int startPtIdx, + int endPtIdx, + std::vector< std::vector>& roiScanLines) +{ + for (int i = startLine; i < endLine; i++) + { + if (i >= scanLines.size()) + break; + + std::vector cut_line; + std::vector& a_line = scanLines[i]; + for (int j = startPtIdx; j < endPtIdx; j++) + { + SVzNL3DPosition a_pt; + if (j >= a_line.size()) + { + a_pt.nPointIdx = 0; + a_pt.pt3D = { 0,0,0 }; + } + else + a_pt = a_line[j]; + cut_line.push_back(a_pt); + } + roiScanLines.push_back(cut_line); + } + return; +} + +#define TEST_COMPUTE_CALIB_PARA 0 +#define TEST_COMPUTE_QRCODE_IMG 1 +#define TEST_GROUP 1 +int main() +{ + const char* dataPath[TEST_GROUP] = { + "F:\\ShangGu\\项目\\工件端部圆点二维码\\", //0 + + }; + + SVzNLRange fileIdx[TEST_GROUP] = { + {3,3} + }; + +#if TEST_COMPUTE_CALIB_PARA + char _calib_datafile[256]; + sprintf_s(_calib_datafile, "F:\\ShangGu\\项目\\工件端部圆点二维码\\LaserLine3_grid.txt"); + int lineNum = 0; + float lineV = 0.0f; + int dataCalib = 0; + int maxTimeStamp = 0; + int clockPerSecond = 0; + SVzNL3DLaserLine* laser3DPoints = vzReadLaserScanPointFromFile_XYZ(_calib_datafile, &lineNum, &lineV, &dataCalib, &maxTimeStamp, &clockPerSecond); + if (laser3DPoints) + { + SSG_planeCalibPara calibPara = wd_getBaseCalibPara( + laser3DPoints, + lineNum); + //结果进行验证 + for (int i = 0; i < lineNum; i++) + { + if (i == 14) + int kkk = 1; + //行处理 + //调平，去除地面 + wd_lineDataR(&laser3DPoints[i], calibPara.planeCalib, -1);// calibPara.planeHeight); + } + // + char calibFile[250]; + sprintf_s(calibFile, "F:\\ShangGu\\项目\\工件端部圆点二维码\\ground_calib_para.txt"); + _outputCalibPara(calibFile, calibPara); + char _out_file[256]; + sprintf_s(_out_file, "F:\\ShangGu\\项目\\工件端部圆点二维码\\LaserLine3_calib_data.txt"); + _outputScanDataFile_ptr(_out_file, laser3DPoints, lineNum); + printf("%s: calib done!\n", _calib_datafile); + } + +#endif + +#if TEST_COMPUTE_QRCODE_IMG + for (int grp = 0; grp <= 0; grp++) + { + int cloud_rows = 4200; + int cloud_cols = 2160; + SSG_planeCalibPara poseCalibPara; + //初始化成单位阵 + poseCalibPara.planeCalib[0] = 1.0; + poseCalibPara.planeCalib[1] = 0.0; + poseCalibPara.planeCalib[2] = 0.0; + poseCalibPara.planeCalib[3] = 0.0; + poseCalibPara.planeCalib[4] = 1.0; + poseCalibPara.planeCalib[5] = 0.0; + poseCalibPara.planeCalib[6] = 0.0; + poseCalibPara.planeCalib[7] = 0.0; + poseCalibPara.planeCalib[8] = 1.0; + poseCalibPara.planeHeight = -1.0; + for (int i = 0; i < 9; i++) + poseCalibPara.invRMatrix[i] = poseCalibPara.planeCalib[i]; + char calibFile[250]; + sprintf_s(calibFile, "F:\\ShangGu\\项目\\工件端部圆点二维码\\ground_calib_para.txt"); + poseCalibPara = _readCalibPara(calibFile); + + for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) + { + //fidx = 1; + char _scan_file[256]; + sprintf_s(_scan_file, "%s%d_Cloud.txt", dataPath[grp], fidx); + std::vector< SVzNL3DPoint> scanData; + bool removeZeros = false; + bool exchangeXY = true; + if (fidx < 3) + { + cloud_cols = 1; //单行测试文件 + exchangeXY = false; + } + else + { + cloud_cols = 2160; //单行测试文件 + exchangeXY = true; + } + if (fidx == 2) + { + vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanData); + cloud_cols = 1100; + cloud_rows = 1300; + if (scanData.size() != (cloud_rows * cloud_cols)) + continue; + } + else + { + vzReadLaserScanPointFromFile_plyTxt(_scan_file, scanData, removeZeros, exchangeXY); + if (scanData.size() != (cloud_rows * cloud_cols)) + continue; + } + //将数据恢复为按扫描线存储格式 + std::vector< std::vector> scanLines; + wd_getScanLines(scanData, scanLines, cloud_rows); + +#if 0 + //点云裁剪 + std::vector< std::vector> roiScanLines; + _getRoiClouds(scanLines, 700, 1800, 400, 1700, roiScanLines); + sprintf_s(_scan_file, "%sLaserLine4_grid.txt", dataPath[grp]); + int headNullLines = 0; + _outputScanDataFile_vector(_scan_file, roiScanLines, false, &headNullLines); +#endif +#if 0 + sprintf_s(_scan_file, "%sLaserLine%d_grid.txt", dataPath[grp], fidx); + int headNullLines = 0; + _outputScanDataFile_vector(_scan_file, scanLines, false, &headNullLines); + printf("%s: head null lines = %d\n", _scan_file, headNullLines); +#endif + WD_QRcodeParam qrcode_param; + qrcode_param.rows = 14; + qrcode_param.cols = 14; + qrcode_param.row_space = 30.0 / 14; + qrcode_param.col_space = 30.0 / 14; + qrcode_param.pointHoleDepth = 1.0; + qrcode_param.pointHoleR = 0.6; + + long t1 = GetTickCount64();//统计时间 + + for (int i = 0, i_max= scanLines.size(); i < i_max; i++) + { + if (i == 14) + int kkk = 1; + //行处理 + //调平，去除地面 + wd_lineDataR(scanLines[i], poseCalibPara.planeCalib, -1); + } + // +#if 0 + sprintf_s(_scan_file, "%sresult\\LaserLine%d_calib_data.txt", dataPath[grp], fidx); + int headNulls = 0; + _outputScanDataFile_vector(_scan_file, scanLines, true, &headNulls); +#endif + cv::Mat dmCodeImg; + std::vector< SVzNL3DPosition> objPoints; + wd_QRcode3Ddetection( + scanLines, + qrcode_param, + objPoints, + dmCodeImg); + + long t2 = GetTickCount64(); + printf("%s: %d(ms)!\n", _scan_file, (int)(t2 - t1)); + //输出测试结果 + sprintf_s(_scan_file, "%sresult\\LaserLine%d_result.txt", dataPath[grp], fidx); + _outputRGBDScanDataFile_RGBD(_scan_file, scanLines, objPoints); + if (!dmCodeImg.empty()) + { + sprintf_s(_scan_file, "%sresult\\LaserLine%d_img.png", dataPath[grp], fidx); + cv::Mat normDmCodeImg; + cv::normalize(dmCodeImg, normDmCodeImg, 0, 255, cv::NORM_MINMAX, CV_8U); + cv::imwrite(_scan_file, normDmCodeImg); + } + } + } +#endif +} + +// 运行程序: Ctrl + F5 或调试 >“开始执行(不调试)”菜单 +// 调试程序: F5 或调试 >“开始调试”菜单 + +// 入门使用技巧: +// 1. 使用解决方案资源管理器窗口添加/管理文件 +// 2. 使用团队资源管理器窗口连接到源代码管理 +// 3. 使用输出窗口查看生成输出和其他消息 +// 4. 使用错误列表窗口查看错误 +// 5. 转到“项目”>“添加新项”以创建新的代码文件，或转到“项目”>“添加现有项”以将现有代码文件添加到项目 +// 6. 将来，若要再次打开此项目，请转到“文件”>“打开”>“项目”并选择 .sln 文件 diff --git a/QRcode3Ddetection_test/QRcode3Ddetection_test.vcxproj b/QRcode3Ddetection_test/QRcode3Ddetection_test.vcxproj new file mode 100644 index 0000000..35f898c --- /dev/null +++ b/QRcode3Ddetection_test/QRcode3Ddetection_test.vcxproj @@ -0,0 +1,157 @@ + + + + + Debug + Win32 + + + Release + Win32 + + + Debug + x64 + + + Release + x64 + + + + 16.0 + Win32Proj + {e5faaacf-703c-4460-93b8-301991f62778} + QRcode3Ddetectiontest + 10.0 + + + + Application + true + v142 + Unicode + + + Application + false + v142 + true + Unicode + + + Application + true + v142 + Unicode + + + Application + false + v142 + true + Unicode + + + + + + + + + + + + + + + + + + + + + true + + + false + + + true + ..\..\thirdParty\VzNLSDK\Inc;..\sourceCode;..\sourceCode\inc;$(IncludePath) + $(SolutionDir)build\$(Platform)\$(Configuration)\ + + + false + ..\..\thirdParty\VzNLSDK\Inc;..\sourceCode;..\sourceCode\inc;$(IncludePath) + $(SolutionDir)build\$(Platform)\$(Configuration)\ + + + + Level3 + true + WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + + + Console + true + + + + + Level3 + true + true + true + WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + + + Console + true + true + true + + + + + Level3 + true + _DEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + ..\..\thirdParty\opencv\build\include; + + + Console + true + ..\..\thirdParty\opencv\build\x64\vc16\lib;..\build\x64\Debug;%(AdditionalLibraryDirectories) + opencv_world480d.lib;QRcode3Ddetection.lib;%(AdditionalDependencies) + + + + + Level3 + true + true + true + NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + ..\..\thirdParty\opencv\build\include; + + + Console + true + true + true + ..\..\thirdParty\opencv\build\x64\vc16\lib;..\build\x64\Release;%(AdditionalLibraryDirectories) + opencv_world480.lib;QRcode3Ddetection.lib;%(AdditionalDependencies) + + + + + + + + + \ No newline at end of file From ae46895573c159107766bfc747a7e37089f6c6f3 Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Tue, 15 Jul 2025 21:37:09 +0800 Subject: [PATCH 2/4] QRCode 3D detection algorithm initial check in --- sourceCode/SG_clustering.cpp | 67 ++ sourceCode/WD_QRcode3Ddetection.cpp | 811 +++++++++++++++++++++++ sourceCode/WD_QRcode3Ddetection_Export.h | 42 ++ 3 files changed, 920 insertions(+) create mode 100644 sourceCode/SG_clustering.cpp create mode 100644 sourceCode/WD_QRcode3Ddetection.cpp create mode 100644 sourceCode/WD_QRcode3Ddetection_Export.h diff --git a/sourceCode/SG_clustering.cpp b/sourceCode/SG_clustering.cpp new file mode 100644 index 0000000..1dcef43 --- /dev/null +++ b/sourceCode/SG_clustering.cpp @@ -0,0 +1,67 @@ +#include "SG_baseDataType.h" +#include "SG_baseAlgo_Export.h" +#include +#include +#include + +void _seedClustering( + std::vector< SVzNL3DPosition>& a_cluster, + std::vector< SVzNL3DPosition>& pts, + double clusterDist) +{ + int i = 0; + while (1) + { + if (i >= a_cluster.size()) + break; + + SVzNL3DPosition a_seed = a_cluster[i]; + for (int i = 0, i_max = pts.size(); i < i_max; i++) + { + if (pts[i].nPointIdx < 0) + continue; + double dist = sqrt(pow(a_seed.pt3D.x - pts[i].pt3D.x, 2) + pow(a_seed.pt3D.y - pts[i].pt3D.y, 2)); + if (dist < clusterDist) + { + a_cluster.push_back(pts[i]); + pts[i].nPointIdx = -1; + } + } + i++; + } + return; +} + +void sg_pointClustering( + std::vector< SVzNL3DPosition>& pts, + double clusterDist, + std::vector>& objClusters //result +) +{ + int ptSize = pts.size(); + if (ptSize == 0) + return; + while(pts.size() > 0) + { + SVzNL3DPosition a_pt = pts[0]; + + //½һcluster + std::vector< SVzNL3DPosition> a_cluster; + a_cluster.push_back(a_pt); + pts[0].nPointIdx = -1; //ֹظ + _seedClustering( + a_cluster, + pts, + clusterDist); + objClusters.push_back(a_cluster); //һ + //ptsдĵȥ + int m_max = pts.size(); + for (int m = m_max - 1; m >= 0; m--) //ӺǰɾӰѭ + { + if(pts[m].nPointIdx < 0) + pts.erase(pts.begin() + m); + } + } + return; +} +; \ No newline at end of file diff --git a/sourceCode/WD_QRcode3Ddetection.cpp b/sourceCode/WD_QRcode3Ddetection.cpp new file mode 100644 index 0000000..1401cef --- /dev/null +++ b/sourceCode/WD_QRcode3Ddetection.cpp @@ -0,0 +1,811 @@ +#include +#include "SG_baseDataType.h" +#include "SG_baseAlgo_Export.h" +#include "WD_QRcode3Ddetection_Export.h" +#include +#include + +typedef struct +{ + int id; + double dist; + double angle; +}_linkInfo; +typedef struct +{ + int objId; + SVzNL3DPoint ptPos; + _linkInfo link[8]; //R, RT, T, LT, L, LB, B, RB +}NodeOctLink; + +typedef struct +{ + SSG_ROIRectD roi; + int* qrCode; +}DMCodeInfo; + +//plyʽݻָɨеʽӶ水нд +void wd_getScanLines( + std::vector& scanData, + std::vector< std::vector>& scanLines, + int scan_rows) +{ + std::vector a_line; + for (int i = 0, i_max = scanData.size(); i < i_max; i++) + { + int idx = i % scan_rows; + if (0 == idx) + { + //µһ + if (a_line.size() > 0) + { + scanLines.push_back(a_line); + a_line.clear(); + } + } + SVzNL3DPoint a_pt = scanData[i]; + SVzNL3DPosition a_idxPt; + a_idxPt.nPointIdx = 0; + a_idxPt.pt3D = a_pt; + a_line.push_back(a_idxPt); + } + if(a_line.size() > 0) + scanLines.push_back(a_line); + return; +} + +SSG_planeCalibPara wd_getBaseCalibPara( + SVzNL3DLaserLine* laser3DPoints, + int lineNum) +{ + return sg_getPlaneCalibPara(laser3DPoints, lineNum); +} + +void wd_lineDataR(std::vector< SVzNL3DPosition>& a_line, + const double* camPoseR, + double groundH) +{ + lineDataRT_vector(a_line, camPoseR, groundH); +} + +SVzNL3DPoint _getObjCenter(SSG_featureTree& a_tree) +{ + SVzNL3DPoint centerPt = { 0, 0, 0 }; + int nodeNum = 0; + for (int i = 0; i < a_tree.treeNodes.size(); i++) + { + centerPt.x += a_tree.treeNodes[i].jumpPos.x; + centerPt.y += a_tree.treeNodes[i].jumpPos.y; + centerPt.z += a_tree.treeNodes[i].jumpPos.z; + nodeNum++; + } + if (nodeNum > 0) + { + centerPt.x = centerPt.x / nodeNum; + centerPt.y = centerPt.y / nodeNum; + centerPt.z = centerPt.z / nodeNum; + } + return centerPt; +} + +_linkInfo compareBestLinl(_linkInfo link_1, _linkInfo link_2, double bestDist) +{ + double dist_diff1 = abs(link_1.dist - bestDist); + double dist_diff2 = abs(link_2.dist - bestDist); + if (dist_diff1 < dist_diff2) + return link_1; + else + return link_2; +} + +//NodeҵLinkϵ +void _createNodeLinks( + std::vector< SVzNL3DPosition>& a_cluster, + std::vector< NodeOctLink>& clusterNodeLinks, + double row_space, + double col_space +) +{ + int dirInvTbl[8] = { 4, 5, 6, 7, 0, 1, 2, 3 }; + double diaDist = sqrt(pow(row_space, 2) + pow(col_space, 2)); + int nodeSize = a_cluster.size(); + for (int i = 0; i < nodeSize; i++) + { + SVzNL3DPosition& a_node = a_cluster[i]; + NodeOctLink& a_nodeLink = clusterNodeLinks[i]; + _linkInfo link[8]; + for (int j = 0; j < 8; j++) + { + link[j].id = -1; + link[j].dist = 0.0; + link[j].angle = 0.0; + } + for (int j = i+1; j < nodeSize; j++) + { + SVzNL3DPosition& chk_node = a_cluster[j]; + NodeOctLink& chk_nodeLink = clusterNodeLinks[j]; + if (chk_node.nPointIdx < 0) + continue; + + double dist = sqrt(pow(a_node.pt3D.x - chk_node.pt3D.x, 2) + pow(a_node.pt3D.y - chk_node.pt3D.y, 2)); + if (dist < diaDist * 2) //ֹ + { + double angle = atan2(a_node.pt3D.y - chk_node.pt3D.y, chk_node.pt3D.x - a_node.pt3D.x); //ͼϵyŷϵô + angle = angle * 180.0 / PI; //תΪǶ + if (angle < 0) + angle += 360; //ת0-360 + + _linkInfo a_link = { j, dist, angle }; + if ((angle > 345) || (angle < 15)) //R:0 + { + if (dist < col_space * 1.5) + { + if (link[0].id < 0) + link[0] = a_link; + else + link[0] = compareBestLinl(link[0], a_link, col_space); + } + } + else if ((angle > 30) && (angle < 60)) //RT:1 + { + if (dist < diaDist * 1.5) + { + if (link[1].id < 0) + link[1] = a_link; + else + link[1] = compareBestLinl(link[1], a_link, diaDist); + } + } + else if ((angle > 75) && (angle < 105)) //T:2 + { + if (dist < row_space * 1.5) + { + if (link[2].id < 0) + link[2] = a_link; + else + link[2] = compareBestLinl(link[2], a_link, row_space); + } + } + else if ((angle > 120) && (angle < 150)) //LT:3 + { + if (dist < diaDist * 1.5) + { + if (link[3].id < 0) + link[3] = a_link; + else + link[3] = compareBestLinl(link[3], a_link, diaDist); + } + } + else if ((angle > 165) && (angle < 195)) //L:4 + { + if (dist < col_space * 1.5) + { + if (link[4].id < 0) + link[4] = a_link; + else + link[4] = compareBestLinl(link[4], a_link, col_space); + } + } + else if ((angle > 210) && (angle < 240)) //LB:5 + { + if (dist < diaDist * 1.5) + { + if (link[5].id < 0) + link[5] = a_link; + else + link[5] = compareBestLinl(link[5], a_link, diaDist); + } + } + else if ((angle > 255) && (angle < 285)) //B:6 + { + if (dist < row_space * 1.5) + { + if (link[6].id < 0) + link[6] = a_link; + else + link[6] = compareBestLinl(link[6], a_link, row_space); + } + } + else if ((angle > 300) && (angle < 330)) //RB:7 + { + if (dist < diaDist * 1.5) + { + if (link[7].id < 0) + link[7] = a_link; + else + link[7] = compareBestLinl(link[7], a_link, diaDist); + } + } + } + } + + for (int j = 0; j < 8; j++) + { + if (link[j].id >= 0) + { + a_nodeLink.link[j] = link[j]; + int linkId = link[j].id; + int dir_inv = dirInvTbl[j]; + if (clusterNodeLinks[linkId].link[dir_inv].id < 0) + { + clusterNodeLinks[linkId].link[dir_inv].id = a_nodeLink.objId; + clusterNodeLinks[linkId].link[dir_inv].dist = link[j].dist; + clusterNodeLinks[linkId].link[dir_inv].angle = link[j].angle + 180; + if (clusterNodeLinks[linkId].link[dir_inv].angle >= 360) + clusterNodeLinks[linkId].link[dir_inv].angle = clusterNodeLinks[linkId].link[dir_inv].angle - 360; + } + } + } + } +} + +// +std::vector _LTseedSearchRight(NodeOctLink& startNode, std::vector< NodeOctLink>& clusterNodeLinks) +{ + NodeOctLink a_seed = startNode; + std::vector result; + result.push_back(a_seed.objId); + while (1) + { + if (a_seed.link[0].id < 0) + break; + a_seed = clusterNodeLinks[a_seed.link[0].id]; + + if ((a_seed.link[1].id < 0) && (a_seed.link[2].id < 0) && (a_seed.link[3].id < 0)) + result.push_back(a_seed.objId); + else + break; + } + return result; +} +std::vector _LTseedSearchBottom(NodeOctLink& startNode, std::vector< NodeOctLink>& clusterNodeLinks) +{ + NodeOctLink a_seed = startNode; + std::vector result; + result.push_back(a_seed.objId); + while (1) + { + if (a_seed.link[6].id < 0) + break; + a_seed = clusterNodeLinks[a_seed.link[6].id]; + + if ((a_seed.link[3].id < 0) && (a_seed.link[4].id < 0) && (a_seed.link[5].id < 0)) + result.push_back(a_seed.objId); + else + break; + } + return result; +} +int _foundDM_LTcorner(std::vector< NodeOctLink>& clusterNodeLinks, int rows, int cols) +{ + for (int i = 0, i_max = clusterNodeLinks.size(); i < i_max; i++) + { + //Ѱ + NodeOctLink a_seed = clusterNodeLinks[i]; + if ((a_seed.link[0].id >= 0) && (a_seed.link[6].id >= 0) && + (a_seed.link[1].id < 0) && (a_seed.link[2].id < 0) && + (a_seed.link[3].id < 0) && (a_seed.link[4].id < 0) && + (a_seed.link[5].id < 0)) //ϸ + { + std::vector R_result = _LTseedSearchRight(a_seed, clusterNodeLinks); + std::vector B_result = _LTseedSearchBottom(a_seed, clusterNodeLinks); + if (((int)R_result.size() == cols) && ((int)B_result.size() == rows)) //ҵLTǵ + return i; + } + } + return -1; +} + +std::vector _RTseedSearchLeft(NodeOctLink& startNode, std::vector< NodeOctLink>& clusterNodeLinks) +{ + NodeOctLink a_seed = startNode; + std::vector result; + result.push_back(a_seed.objId); + while (1) + { + if (a_seed.link[4].id < 0) + break; + a_seed = clusterNodeLinks[a_seed.link[4].id]; + + if ((a_seed.link[1].id < 0) && (a_seed.link[2].id < 0) && (a_seed.link[3].id < 0)) + result.push_back(a_seed.objId); + else + break; + } + return result; +} +std::vector _RTseedSearchBottom(NodeOctLink& startNode, std::vector< NodeOctLink>& clusterNodeLinks) +{ + NodeOctLink a_seed = startNode; + std::vector result; + result.push_back(a_seed.objId); + while (1) + { + if (a_seed.link[6].id < 0) + break; + a_seed = clusterNodeLinks[a_seed.link[6].id]; + + if ((a_seed.link[1].id < 0) && (a_seed.link[0].id < 0) && (a_seed.link[7].id < 0)) + result.push_back(a_seed.objId); + else + break; + } + return result; +} +int _foundDM_RTcorner(std::vector< NodeOctLink>& clusterNodeLinks, int rows, int cols) +{ + for (int i = 0, i_max = clusterNodeLinks.size(); i < i_max; i++) + { + //Ѱ + NodeOctLink a_seed = clusterNodeLinks[i]; + if ((a_seed.link[4].id >= 0) && (a_seed.link[6].id >= 0) && + (a_seed.link[0].id < 0) && (a_seed.link[1].id < 0) && + (a_seed.link[2].id < 0) && (a_seed.link[3].id < 0) && + (a_seed.link[7].id < 0)) //ϸ + { + std::vector L_result = _RTseedSearchLeft(a_seed, clusterNodeLinks); + std::vector B_result = _RTseedSearchBottom(a_seed, clusterNodeLinks); + if (((int)L_result.size() == cols) && ((int)B_result.size() == rows)) //ҵLTǵ + return i; + } + } + return -1; +} + +std::vector _LBseedSearchRight(NodeOctLink& startNode, std::vector< NodeOctLink>& clusterNodeLinks) +{ + NodeOctLink a_seed = startNode; + std::vector result; + result.push_back(a_seed.objId); + while (1) + { + if (a_seed.link[0].id < 0) + break; + a_seed = clusterNodeLinks[a_seed.link[0].id]; + + if ((a_seed.link[5].id < 0) && (a_seed.link[6].id < 0) && (a_seed.link[7].id < 0)) + result.push_back(a_seed.objId); + else + break; + } + return result; +} +std::vector _LBseedSearchTop(NodeOctLink& startNode, std::vector< NodeOctLink>& clusterNodeLinks) +{ + NodeOctLink a_seed = startNode; + std::vector result; + result.push_back(a_seed.objId); + while (1) + { + if (a_seed.link[2].id < 0) + break; + a_seed = clusterNodeLinks[a_seed.link[2].id]; + + if ((a_seed.link[3].id < 0) && (a_seed.link[4].id < 0) && (a_seed.link[5].id < 0)) + result.push_back(a_seed.objId); + else + break; + } + return result; +} +int _foundDM_LBcorner(std::vector< NodeOctLink>& clusterNodeLinks, int rows, int cols) +{ + for (int i = 0, i_max = clusterNodeLinks.size(); i < i_max; i++) + { + //Ѱ + NodeOctLink a_seed = clusterNodeLinks[i]; + if ((a_seed.link[0].id >= 0) && (a_seed.link[2].id >= 0) && + (a_seed.link[3].id < 0) && (a_seed.link[4].id < 0) && + (a_seed.link[5].id < 0) && (a_seed.link[6].id < 0) && + (a_seed.link[7].id < 0)) //ϸ + { + std::vector L_result = _LBseedSearchRight(a_seed, clusterNodeLinks); + std::vector B_result = _LBseedSearchTop(a_seed, clusterNodeLinks); + if (((int)L_result.size() == cols) && ((int)B_result.size() == rows)) //ҵLTǵ + return i; + } + } + return -1; +} + +std::vector _RBseedSearchLeft(NodeOctLink& startNode, std::vector< NodeOctLink>& clusterNodeLinks) +{ + NodeOctLink a_seed = startNode; + std::vector result; + result.push_back(a_seed.objId); + while (1) + { + if (a_seed.link[4].id < 0) + break; + a_seed = clusterNodeLinks[a_seed.link[4].id]; + + if ((a_seed.link[5].id < 0) && (a_seed.link[6].id < 0) && (a_seed.link[7].id < 0)) + result.push_back(a_seed.objId); + else + break; + } + return result; +} +std::vector _RBseedSearchTop(NodeOctLink& startNode, std::vector< NodeOctLink>& clusterNodeLinks) +{ + NodeOctLink a_seed = startNode; + std::vector result; + result.push_back(a_seed.objId); + while (1) + { + if (a_seed.link[2].id < 0) + break; + a_seed = clusterNodeLinks[a_seed.link[2].id]; + + if ((a_seed.link[1].id < 0) && (a_seed.link[0].id < 0) && (a_seed.link[7].id < 0)) + result.push_back(a_seed.objId); + else + break; + } + return result; +} +int _foundDM_RBcorner(std::vector< NodeOctLink>& clusterNodeLinks, int rows, int cols) +{ + for (int i = 0, i_max = clusterNodeLinks.size(); i < i_max; i++) + { + //Ѱ + NodeOctLink a_seed = clusterNodeLinks[i]; + if ((a_seed.ptPos.x > 19.2) && (a_seed.ptPos.y > -31)) + int kkk = 1; + if ((a_seed.link[2].id >= 0) && (a_seed.link[4].id >= 0) && + (a_seed.link[0].id < 0) && (a_seed.link[1].id < 0) && + (a_seed.link[5].id < 0) && (a_seed.link[6].id < 0) && + (a_seed.link[7].id < 0)) //ϸ + { + std::vector L_result = _RBseedSearchLeft(a_seed, clusterNodeLinks); + std::vector B_result = _RBseedSearchTop(a_seed, clusterNodeLinks); + if (((int)L_result.size() == cols) && ((int)B_result.size() == rows)) //ҵLTǵ + return i; + } + } + return -1; +} + +typedef struct +{ + int idx; + SVzNL2DPoint pt2D; +}DMPos; + +void _getDMPosLinks(std::vector< DMPos>& posLink, std::vector< NodeOctLink>& clusterNodeLinks) +{ + int i = 0; + while (1) + { + if (i >= posLink.size()) + break; + DMPos a_seed = posLink[i]; + NodeOctLink& a_node = clusterNodeLinks[a_seed.idx]; + if (a_node.objId >= 0) + { + if (a_node.link[0].id >= 0) + { + DMPos new_seed; + new_seed.idx = a_node.link[0].id; + new_seed.pt2D = { a_seed.pt2D.x + 1, a_seed.pt2D.y }; + if (clusterNodeLinks[new_seed.idx].objId >= 0) + { + clusterNodeLinks[new_seed.idx].link[4].id = -1; //Ѿ + posLink.push_back(new_seed); + } + } + if (a_node.link[1].id >= 0) + { + DMPos new_seed; + new_seed.idx = a_node.link[1].id; + new_seed.pt2D = { a_seed.pt2D.x + 1, a_seed.pt2D.y - 1 }; + if (clusterNodeLinks[new_seed.idx].objId >= 0) + { + clusterNodeLinks[new_seed.idx].link[5].id = -1; //Ѿ + posLink.push_back(new_seed); + } + } + if (a_node.link[2].id >= 0) + { + DMPos new_seed; + new_seed.idx = a_node.link[2].id; + new_seed.pt2D = { a_seed.pt2D.x, a_seed.pt2D.y - 1 }; + if (clusterNodeLinks[new_seed.idx].objId >= 0) + { + clusterNodeLinks[new_seed.idx].link[6].id = -1; //Ѿ + posLink.push_back(new_seed); + } + } + if (a_node.link[3].id >= 0) + { + DMPos new_seed; + new_seed.idx = a_node.link[3].id; + new_seed.pt2D = { a_seed.pt2D.x - 1, a_seed.pt2D.y - 1 }; + clusterNodeLinks[new_seed.idx].link[7].id = -1; //Ѿ + posLink.push_back(new_seed); + } + if (a_node.link[4].id >= 0) + { + DMPos new_seed; + new_seed.idx = a_node.link[4].id; + new_seed.pt2D = { a_seed.pt2D.x - 1, a_seed.pt2D.y }; + if (clusterNodeLinks[new_seed.idx].objId >= 0) + { + clusterNodeLinks[new_seed.idx].link[0].id = -1; //Ѿ + posLink.push_back(new_seed); + } + } + if (a_node.link[5].id >= 0) + { + DMPos new_seed; + new_seed.idx = a_node.link[5].id; + new_seed.pt2D = { a_seed.pt2D.x - 1, a_seed.pt2D.y + 1 }; + if (clusterNodeLinks[new_seed.idx].objId >= 0) + { + clusterNodeLinks[new_seed.idx].link[1].id = -1; //Ѿ + posLink.push_back(new_seed); + } + } + if (a_node.link[6].id >= 0) + { + DMPos new_seed; + new_seed.idx = a_node.link[6].id; + new_seed.pt2D = { a_seed.pt2D.x, a_seed.pt2D.y + 1 }; + if (clusterNodeLinks[new_seed.idx].objId >= 0) + { + clusterNodeLinks[new_seed.idx].link[2].id = -1; //Ѿ + posLink.push_back(new_seed); + } + } + if (a_node.link[7].id >= 0) + { + DMPos new_seed; + new_seed.idx = a_node.link[7].id; + new_seed.pt2D = { a_seed.pt2D.x + 1, a_seed.pt2D.y + 1 }; + if (clusterNodeLinks[new_seed.idx].objId >= 0) + { + clusterNodeLinks[new_seed.idx].link[3].id = -1; //Ѿ + posLink.push_back(new_seed); + } + } + clusterNodeLinks[a_seed.idx].objId = -1; //Ѿ + } + i++; + } + return; +} + +SSG_ROIRectD _getClusterROI(std::vector< SVzNL3DPosition>& a_cluster) +{ + SSG_ROIRectD roi = {0,0,0,0}; + int nodeSize = a_cluster.size(); + if (nodeSize == 0) + return roi; + + roi = { a_cluster [0].pt3D.x, a_cluster[0].pt3D.x, a_cluster[0].pt3D.y, a_cluster[0].pt3D.y}; + for (int i = 1; i < nodeSize; i++) + { + roi.left = roi.left > a_cluster[i].pt3D.x ? a_cluster[i].pt3D.x : roi.left; + roi.right = roi.right < a_cluster[i].pt3D.x ? a_cluster[i].pt3D.x : roi.right; + roi.top = roi.top > a_cluster[i].pt3D.y ? a_cluster[i].pt3D.y : roi.top; + roi.bottom = roi.bottom < a_cluster[i].pt3D.y ? a_cluster[i].pt3D.y : roi.bottom; + } + return roi; +} + +void wd_QRcode3Ddetection( + std::vector< std::vector>& scanLines, + const WD_QRcodeParam qrcode_param, + std::vector< SVzNL3DPosition>& outObjPoints, + cv::Mat& dmCodeImg) +{ + std::vector> lineZMaxPts; + int lineNum = scanLines.size(); + + double scaleWin = qrcode_param.row_space / 5; + double minPkHeighth = qrcode_param.pointHoleDepth / 2; + double holeR = qrcode_param.pointHoleR; + for (int line = 0; line < lineNum; line++) + { + std::vector< SSG_basicFeature1D> a_lineMax; + sg_getFlatLineLocalPeaks_vector( + scanLines[line], + line, + scaleWin, + minPkHeighth, + holeR, + a_lineMax); + lineZMaxPts.push_back(a_lineMax); + } + // + SSG_treeGrowParam growParam; + growParam.yDeviation_max = holeR;//ʱYƫ + growParam.zDeviation_max = holeR; //ʱZƫ + growParam.maxLineSkipNum = 5; //ʱ߼ -1ʱʹmaxDkipDistance + growParam.maxSkipDistance = -1; //maxLineSkipNumΪ-1 ʹô˲.Ϊ-1ʱ˲Ч + growParam.minLTypeTreeLen = holeR/2; //ٵĽڵĿСڴĿƳ + growParam.minVTypeTreeLen = holeR/2; //ٵĽڵĿСڴĿƳ + + std::vector trees; + for (int line = 0; line < lineNum; line++) + { + std::vector< SSG_basicFeature1D>& a_lineMax = lineZMaxPts[line]; + if (a_lineMax.size() == 0) + continue; + + //feature + bool isLastLine = false; + if (line == lineNum - 1) + isLastLine = true; + sg_lineFeaturesGrowing( + line, + isLastLine, + a_lineMax, + trees, + growParam); + } + + //Ŀĵ㣬Դ˴ + std::vector< SVzNL3DPosition> objPoints; + objPoints.resize(trees.size()); + for (int i = 0, i_max = trees.size(); i < i_max; i++) + { + SVzNL3DPosition a_obj; + a_obj.nPointIdx = i; + a_obj.pt3D = _getObjCenter(trees[i]); + objPoints[i] = a_obj; + } + //ڽĵϲ + int objPtSize = objPoints.size(); + for (int i = objPtSize - 1; i >= 0; i--) + { + SVzNL3DPosition a_pt = objPoints[i]; + bool isCombined = false; + for (int j = i - 1; j >= 0; j--) + { + SVzNL3DPosition chk_pt = objPoints[j]; + double dist = sqrt(pow(a_pt.pt3D.x - chk_pt.pt3D.x, 2) + pow(a_pt.pt3D.y - chk_pt.pt3D.y, 2)); + if (dist < holeR * 2) //ϲ + { + objPoints[j].pt3D.x = (a_pt.pt3D.x + chk_pt.pt3D.x) / 2; + objPoints[j].pt3D.y = (a_pt.pt3D.y + chk_pt.pt3D.y) / 2; + objPoints[j].pt3D.z = (a_pt.pt3D.z + chk_pt.pt3D.z) / 2; + isCombined = true; + break; + } + } + if (true == isCombined) + objPoints.erase(objPoints.begin() + i); + } + for (int i = 0, i_max = objPoints.size(); i < i_max; i++) + outObjPoints.push_back(objPoints[i]); + + //ֳ࣬һά + double clusterDist = (qrcode_param.row_space + qrcode_param.col_space) * 2; //ʹ̸ + std::vector> objClusters; + sg_pointClustering( + objPoints, + clusterDist, + objClusters //result + ); + + std::vector qrCodeResult; + for (int i = 0, i_max = objClusters.size(); i < i_max; i++) + { + std::vector< SVzNL3DPosition>& a_cluster = objClusters[i]; + int clustNodeSize = a_cluster.size(); + //ڹϵ + std::vector< NodeOctLink> clusterNodeLinks; + clusterNodeLinks.resize(clustNodeSize); + for (int m = 0; m < clustNodeSize; m++) //ʼ + { + clusterNodeLinks[m].objId = m; + clusterNodeLinks[m].ptPos = a_cluster[m].pt3D; + for (int j = 0; j < 8; j++) + { + clusterNodeLinks[m].link[j].id = -1; + clusterNodeLinks[m].link[j].dist = 0.0; + clusterNodeLinks[m].link[j].angle = 0.0; + } + } + _createNodeLinks(a_cluster, clusterNodeLinks, qrcode_param.row_space, qrcode_param.col_space); + //ѰDMά + int cornerIdx = 0; + int cornerNodeIdx = _foundDM_LTcorner(clusterNodeLinks, qrcode_param.rows, qrcode_param.cols); + if (cornerNodeIdx < 0) + { + cornerIdx = 1; + cornerNodeIdx = _foundDM_RTcorner(clusterNodeLinks, qrcode_param.rows, qrcode_param.cols); + if (cornerNodeIdx < 0) + { + cornerIdx = 2; + cornerNodeIdx = _foundDM_LBcorner(clusterNodeLinks, qrcode_param.rows, qrcode_param.cols); + if (cornerNodeIdx < 0) + { + cornerIdx = 3; + cornerNodeIdx = _foundDM_RBcorner(clusterNodeLinks, qrcode_param.rows, qrcode_param.cols); + } + } + } + if (cornerNodeIdx >= 0) + { + //ȡάֵ + int rows = qrcode_param.rows; + int cols = qrcode_param.cols; + + DMPos startPos; + startPos.idx = cornerNodeIdx; + if (cornerIdx == 0) //LT + startPos.pt2D = { 0,0 }; + else if (cornerIdx == 1) //RT + startPos.pt2D = { 0,cols-1 }; + else if(cornerIdx == 2) //LB + startPos.pt2D = { rows-1,0 }; + else //RB + startPos.pt2D = { rows - 1, cols-1 }; + + std::vector< DMPos> posLinks; + posLinks.push_back(startPos); + _getDMPosLinks(posLinks, clusterNodeLinks); + + int* codeBuff = (int*)malloc(sizeof(int) * rows * cols); + memset(codeBuff, 0, sizeof(int) * qrcode_param.rows * qrcode_param.cols); + for (int m = 0, m_max = posLinks.size(); m < m_max; m++) + { + int col = posLinks[m].pt2D.x; + int row = posLinks[m].pt2D.y; + codeBuff[row * cols + col] = 1; + } + // + DMCodeInfo a_dmCode; + a_dmCode.roi = _getClusterROI(a_cluster); + a_dmCode.qrCode = codeBuff; + qrCodeResult.push_back(a_dmCode); + } + } + if (qrCodeResult.size() > 0) + { + SVzNL3DRangeD globalRoi = sg_getScanDataROI_vector(scanLines); + //ָάͼ + int dmNodeSize = 60; + int dmLen = dmNodeSize * qrcode_param.cols; + double scale = (qrCodeResult[0].roi.right - qrCodeResult[0].roi.left) / (double)dmLen; + int skip = dmNodeSize; + int img_cols = (int)((globalRoi.xRange.max - globalRoi.xRange.min) / scale); + img_cols = ((img_cols + 3) / 4) * 4; //4 + int img_rows = (int)((globalRoi.yRange.max - globalRoi.yRange.min) / scale); + img_rows = ((img_rows + 3) / 4) * 4; //4 + //Ͽհױ + img_cols += skip * 2; + img_rows += skip * 2; + dmCodeImg = cv::Mat::ones(img_rows, img_cols, CV_8UC1); + for (int i = 0; i < qrCodeResult.size(); i++) + { + double x = qrCodeResult[i].roi.left; + double y = qrCodeResult[i].roi.top; + SVzNL2DPoint cornerPos; + cornerPos.x = (int)((x-globalRoi.xRange.min) / scale) + skip; + cornerPos.y = (int)((y-globalRoi.yRange.min) / scale) + skip; + for (int row = 0; row < qrcode_param.rows; row++) + { + for (int col = 0; col < qrcode_param.cols; col++) + { + SVzNL2DPoint nodePos; + nodePos.x = cornerPos.x + col * dmNodeSize; + nodePos.y = cornerPos.y + row * dmNodeSize; + if (qrCodeResult[i].qrCode[row * qrcode_param.cols + col] > 0) + { + for (int m = -dmNodeSize / 2; m < dmNodeSize / 2; m++) + { + for (int n = -dmNodeSize / 2; n < dmNodeSize / 2; n++) + dmCodeImg.at(nodePos.y + m, nodePos.x + n) = 0; + } + } + } + } + free(qrCodeResult[i].qrCode); + qrCodeResult[i].qrCode = NULL; + } + } + return; +} \ No newline at end of file diff --git a/sourceCode/WD_QRcode3Ddetection_Export.h b/sourceCode/WD_QRcode3Ddetection_Export.h new file mode 100644 index 0000000..de2a2ca --- /dev/null +++ b/sourceCode/WD_QRcode3Ddetection_Export.h @@ -0,0 +1,42 @@ +#pragma once + +#if defined(SG_API_LIBRARY) +# define SG_APISHARED_EXPORT __declspec(dllexport) +#else +# define SG_APISHARED_EXPORT __declspec(dllimport) +#endif + +#include "SG_baseDataType.h" +#include +#include + +typedef struct +{ + int rows; //ά + int cols; //ά + double row_space; //м࣬λmm + double col_space; //м࣬λmm + double pointHoleDepth; //άȣλmm + double pointHoleR; //άİ뾶λmm +}WD_QRcodeParam; + +//plyʽݻָɨеʽӶ水нд +SG_APISHARED_EXPORT void wd_getScanLines( + std::vector& scanData, + std::vector< std::vector>& scanLines, + int scan_rows); + +SG_APISHARED_EXPORT SSG_planeCalibPara wd_getBaseCalibPara( + SVzNL3DLaserLine* laser3DPoints, + int lineNum); + +SG_APISHARED_EXPORT void wd_lineDataR( + std::vector< SVzNL3DPosition>& a_line, + const double* camPoseR, + double groundH); + +SG_APISHARED_EXPORT void wd_QRcode3Ddetection( + std::vector< std::vector>& scanLines, + const WD_QRcodeParam qrcode_param, + std::vector< SVzNL3DPosition>& objPoints, + cv::Mat& dmCodeImg); \ No newline at end of file From 497b98e2c35f4235b1b4b3255de2be0d3632e304 Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Tue, 15 Jul 2025 21:39:09 +0800 Subject: [PATCH 3/4] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E4=BA=86=E6=8B=86?= =?UTF-8?q?=E5=8C=85=E7=AE=97=E6=B3=95=E4=B8=AD=E7=BC=96=E7=BB=87=E8=A2=8B?= =?UTF-8?q?=E6=AD=A3=E5=8F=8D=E8=AF=86=E5=88=AB=E5=92=8C=E6=9C=9D=E5=90=91?= =?UTF-8?q?=E8=AF=86=E5=88=AB=E7=AE=97=E6=B3=95=EF=BC=8C=E4=BB=A5=E5=8F=8A?= =?UTF-8?q?=E7=9B=B8=E5=BA=94=E7=9A=84=E6=B5=8B=E8=AF=95=E7=A8=8B=E5=BA=8F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- SG_Algorithm.sln | 26 + .../SieveNodeDetection_test.cpp | 218 +++- bagPositioning_test/bagPositioning_test.cpp | 1143 ++++++++++++++++- baseAlgorithm/baseAlgorithm.vcxproj | 1 + sourceCode/SG_bagPositioning.cpp | 443 ++++++- sourceCode/SG_bagPositioning_Export.h | 38 +- sourceCode/SG_baseAlgo_Export.h | 35 +- sourceCode/SG_baseDataType.h | 22 + sourceCode/SG_baseFunc.cpp | 120 +- sourceCode/SG_featureGrow.cpp | 48 + sourceCode/SG_lineFeature.cpp | 803 +++++++++++- 11 files changed, 2793 insertions(+), 104 deletions(-) diff --git a/SG_Algorithm.sln b/SG_Algorithm.sln index 0527c64..b41a680 100644 --- a/SG_Algorithm.sln +++ b/SG_Algorithm.sln @@ -66,6 +66,16 @@ Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "SieveNodeDetection_test", " {D47DE411-B18C-4A64-BDB9-0AFAB50E8A8A} = {D47DE411-B18C-4A64-BDB9-0AFAB50E8A8A} EndProjectSection EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "QRcode3Ddetection", "QRcode3Ddetection\QRcode3Ddetection.vcxproj", "{A7C72ED9-B81D-4C6A-BE38-D75199C824AA}" + ProjectSection(ProjectDependencies) = postProject + {95DC3F1A-902A-490E-BD3B-B10463CF0EBD} = {95DC3F1A-902A-490E-BD3B-B10463CF0EBD} + EndProjectSection +EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "QRcode3Ddetection_test", "QRcode3Ddetection_test\QRcode3Ddetection_test.vcxproj", "{E5FAAACF-703C-4460-93B8-301991F62778}" + ProjectSection(ProjectDependencies) = postProject + {A7C72ED9-B81D-4C6A-BE38-D75199C824AA} = {A7C72ED9-B81D-4C6A-BE38-D75199C824AA} + EndProjectSection +EndProject Global GlobalSection(SolutionConfigurationPlatforms) = preSolution Debug|x64 = Debug|x64 @@ -178,6 +188,22 @@ Global {DF0881C7-43D0-4765-97CA-74C126665F3E}.Release|x64.Build.0 = Release|x64 {DF0881C7-43D0-4765-97CA-74C126665F3E}.Release|x86.ActiveCfg = Release|Win32 {DF0881C7-43D0-4765-97CA-74C126665F3E}.Release|x86.Build.0 = Release|Win32 + {A7C72ED9-B81D-4C6A-BE38-D75199C824AA}.Debug|x64.ActiveCfg = Debug|x64 + {A7C72ED9-B81D-4C6A-BE38-D75199C824AA}.Debug|x64.Build.0 = Debug|x64 + {A7C72ED9-B81D-4C6A-BE38-D75199C824AA}.Debug|x86.ActiveCfg = Debug|Win32 + {A7C72ED9-B81D-4C6A-BE38-D75199C824AA}.Debug|x86.Build.0 = Debug|Win32 + {A7C72ED9-B81D-4C6A-BE38-D75199C824AA}.Release|x64.ActiveCfg = Release|x64 + {A7C72ED9-B81D-4C6A-BE38-D75199C824AA}.Release|x64.Build.0 = Release|x64 + {A7C72ED9-B81D-4C6A-BE38-D75199C824AA}.Release|x86.ActiveCfg = Release|Win32 + {A7C72ED9-B81D-4C6A-BE38-D75199C824AA}.Release|x86.Build.0 = Release|Win32 + {E5FAAACF-703C-4460-93B8-301991F62778}.Debug|x64.ActiveCfg = Debug|x64 + {E5FAAACF-703C-4460-93B8-301991F62778}.Debug|x64.Build.0 = Debug|x64 + {E5FAAACF-703C-4460-93B8-301991F62778}.Debug|x86.ActiveCfg = Debug|Win32 + {E5FAAACF-703C-4460-93B8-301991F62778}.Debug|x86.Build.0 = Debug|Win32 + {E5FAAACF-703C-4460-93B8-301991F62778}.Release|x64.ActiveCfg = Release|x64 + {E5FAAACF-703C-4460-93B8-301991F62778}.Release|x64.Build.0 = Release|x64 + {E5FAAACF-703C-4460-93B8-301991F62778}.Release|x86.ActiveCfg = Release|Win32 + {E5FAAACF-703C-4460-93B8-301991F62778}.Release|x86.Build.0 = Release|Win32 EndGlobalSection GlobalSection(SolutionProperties) = preSolution HideSolutionNode = FALSE diff --git a/SieveNodeDetection_test/SieveNodeDetection_test.cpp b/SieveNodeDetection_test/SieveNodeDetection_test.cpp index 29704d4..ddd7670 100644 --- a/SieveNodeDetection_test/SieveNodeDetection_test.cpp +++ b/SieveNodeDetection_test/SieveNodeDetection_test.cpp @@ -484,7 +484,7 @@ typedef struct int b; }SG_color; void _outputScanDataFile_RGBD_obj_XYZ(char* fileName, SVzNL3DLaserLine* scanData, int lineNum, - float lineV, int maxTimeStamp, int clockPerSecond, std::vector& objOps) + float lineV, int maxTimeStamp, int clockPerSecond, std::vector>& objOps) { std::ofstream sw(fileName); int realLines = lineNum; @@ -605,35 +605,46 @@ void _outputScanDataFile_RGBD_obj_XYZ(char* fileName, SVzNL3DLaserLine* scanData } if (objOps.size() > 0) { - int ptNum = objOps.size(); - sw << "Line_" << lineNum << "_" << (nTimeStamp + 1000) << "_" << ptNum << std::endl; - for (int i = 0; i < objOps.size(); i++) + int objNum = 0; + for (int i = 0, i_max = objOps.size(); i < i_max; i++) { - if (i == 0) - rgb = { 255, 0, 0 }; - else - rgb = { 255, 255, 0 }; - size = 25; - float x = (float)objOps[i].x; - float y = (float)objOps[i].y; - float z = (float)objOps[i].z; + std::vector& obj_line = objOps[i]; + objNum += obj_line.size(); + } - sw << "{" << x << "," << y << "," << z << "}-"; - sw << "{0,0}-{0,0}-"; - sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; - if (i == 0) + sw << "Line_" << lineNum << "_" << (nTimeStamp + 1000) << "_" << objNum << std::endl; + for (int i = 0, i_max = objOps.size(); i < i_max; i++) + { + std::vector& obj_line = objOps[i]; + for (int j = 0, j_max = obj_line.size(); j < j_max; j++) { + rgb = objColor[i%8]; + size = 25; + float x = (float)obj_line[j].x; + float y = (float)obj_line[j].y; + float z = (float)obj_line[j].z; + sw << "{" << x << "," << y << "," << z << "}-"; sw << "{0,0}-{0,0}-"; sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; + if (i == 0) + { + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; + } } } } sw.close(); } -void _XOYprojection_nodePos(cv::Mat& img, std::vector>& dataLines, std::vector& objOps, - const double x_scale, const double y_scale, const SVzNLRangeD x_range, const SVzNLRangeD y_range) +void _XOYprojection_nodePos( + cv::Mat& img, + std::vector>& dataLines, + std::vector>& objOps, + const double x_scale, const double y_scale, + const SVzNLRangeD x_range, const SVzNLRangeD y_range) { int x_skip = 16; int y_skip = 16; @@ -748,27 +759,24 @@ void _XOYprojection_nodePos(cv::Mat& img, std::vector 0) { - for (int i = 0; i < objOps.size(); i++) + for (int i = 0, i_max = objOps.size(); i < i_max; i++) { - if (i == 0) + std::vector& obj_line = objOps[i]; + for (int j = 0, j_max = obj_line.size(); j < j_max; j++) { - rgb = { 255, 0, 0 }; + rgb = objColor[i % 8]; size = 20; + + int px = (int)((obj_line[j].x - x_range.min) / x_scale + x_skip); + int py = (int)((obj_line[j].y - y_range.min) / y_scale + y_skip); + cv::circle(img, cv::Point(px, py), size, cv::Scalar(rgb[2], rgb[1], rgb[0]), -1); } - else - { - rgb = { 255, 255, 0 }; - size = 10; - } - int px = (int)((objOps[i].x - x_range.min) / x_scale + x_skip); - int py = (int)((objOps[i].y - y_range.min) / y_scale + y_skip); - cv::circle(img, cv::Point(px, py), size, cv::Scalar(rgb[2], rgb[1], rgb[0]), -1); } } } -void _genXOYProjectionImage_nodePos(cv::String& fileName, SVzNL3DLaserLine* scanData, int lineNum, std::vector& nodeOps) +void _genXOYProjectionImage_nodePos(cv::String& fileName, SVzNL3DLaserLine* scanData, int lineNum, std::vector>& nodeOps) { //统计X和Y的范围 std::vector> scan_lines; @@ -911,12 +919,160 @@ SSG_planeCalibPara _readCalibPara(char* fileName) return planePara; } +std::vector _wdReadLMIData_TXT(char* fileName) +{ + std::ifstream inputFile(fileName); + std::string linedata; + + std::vector resultData; + if (inputFile.is_open() == false) + return resultData; + + while (getline(inputFile, linedata)) + { + double x, y, z; + sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &y, &x, &z); + SVzNL3DPoint a_pt; + a_pt.x = x; + a_pt.y = y; + a_pt.z = z; + resultData.push_back(a_pt); + } + return resultData; +} + +std::vector _wdReadGuangZiData_TXT(char* fileName, double tick_scale, double offset) +{ + std::ifstream inputFile(fileName); + std::string linedata; + + std::vector resultData; + if (inputFile.is_open() == false) + return resultData; + + while (getline(inputFile, linedata)) + { + double x, y, z; + sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &x, &y, &z); + if (z > -9999.0 + 1e-4) //1e-4为防止float误差 + { + SVzNL3DPoint a_pt; + a_pt.x = (x - offset)* tick_scale; + a_pt.y = y; + a_pt.z = z; + resultData.push_back(a_pt); + } + } + return resultData; +} + +SVzNL3DLaserLine* _convertToVizumDataFormat(std::vector& ptList, int* vldLineNum) +{ + + std::vector> lineList; + std::vector< SVzNL3DPoint> a_line; + double pre_y = 1.0e+10;; + int maxLineSize = 0; + for (int i = 0, i_max = ptList.size(); i < i_max; i++) + { + if (ptList[i].y < pre_y-1.0) //1.0为容错门限，防止相邻点有反转 + { + //新行开始 + if (a_line.size() > 0) + { + lineList.push_back(a_line); + if (maxLineSize < a_line.size()) + maxLineSize = a_line.size(); + a_line.clear(); + } + a_line.push_back(ptList[i]); + } + else + a_line.push_back(ptList[i]); + pre_y = ptList[i].y; + } + //最后一行 + if (a_line.size() > 0) + { + lineList.push_back(a_line); + if (maxLineSize < a_line.size()) + maxLineSize = a_line.size(); + } + + int lineNum = lineList.size(); + *vldLineNum = lineNum; + SVzNL3DLaserLine* resultdData = (SVzNL3DLaserLine*)malloc(sizeof(SVzNL3DLaserLine) * lineNum); + memset(resultdData, 0, sizeof(SVzNL3DLaserLine) * lineNum); + for (int line = 0; line < lineNum; line++) + { + std::vector< SVzNL3DPoint>& a_lineData = lineList[line]; + int pt_counter = a_lineData.size(); + resultdData[line].nPositionCnt = pt_counter; + resultdData[line].nTimeStamp = 0; + resultdData[line].p3DPosition = (SVzNL3DPosition*)malloc(sizeof(SVzNL3DPosition) * pt_counter); + memset(resultdData[line].p3DPosition, 0, sizeof(SVzNL3DPosition) * pt_counter); + for (int i = 0; i < pt_counter; i++) + { + resultdData[line].p3DPosition[i].nPointIdx = i; + resultdData[line].p3DPosition[i].pt3D = a_lineData[i]; + } + } + return resultdData; +} + + #define TEST_CONVERT_TO_GRID 0 #define TEST_COMPUTE_SIEVE_NODES 1 #define TEST_COMPUTE_CALIB_PARA 0 +#define TEST_CONVERT_LMI_DATA 0 +#define TEST_CONVERT_GUANGZI_DATA 0 #define TEST_GROUP 2 int main() { +#if TEST_CONVERT_LMI_DATA + //将数据转换成栅格格式格式 + int lineNum = 0; + float lineV = 0.0f; + int dataCalib = 0; + int maxTimeStamp = 0; + int clockPerSecond = 0; + char _LMI_file[256]; + sprintf_s(_LMI_file, "F:\\张奔-标定采图\\钢块-标定采图\\5-右下-2.txt"); + std::vector LMI_data = _wdReadLMIData_TXT(_LMI_file); + if (LMI_data.size() == 0) + return 0; + + int vldLineNum = 0; + SVzNL3DLaserLine* convertData = _convertToVizumDataFormat(LMI_data, &vldLineNum); + char _out_file[256]; + sprintf_s(_out_file, "F:\\张奔-标定采图\\钢块-标定采图\\5-右下-2_onverted.txt"); + _outputScanDataFile_self(_out_file, convertData, vldLineNum, + lineV, maxTimeStamp, clockPerSecond); + printf("%s: convert done!\n", _LMI_file); +#endif +#if TEST_CONVERT_GUANGZI_DATA + //将数据转换成栅格格式格式 + int lineNum = 0; + float lineV = 0.0f; + int dataCalib = 0; + int maxTimeStamp = 0; + int clockPerSecond = 0; + char _Guangzi_file[256]; + double offset = 300000; + double tick_scale = 0.05; //左正 + sprintf_s(_Guangzi_file, "F:\\张奔-标定采图\\钢块-标定采图\\5-右正-2.txt"); + std::vector Guangzi_data = _wdReadGuangZiData_TXT(_Guangzi_file, tick_scale, offset); + if (Guangzi_data.size() == 0) + return 0; + + int vldLineNum = 0; + SVzNL3DLaserLine* convertData = _convertToVizumDataFormat(Guangzi_data, &vldLineNum); + char _out_file[256]; + sprintf_s(_out_file, "F:\\张奔-标定采图\\钢块-标定采图\\5-右正-2_onverted.txt"); + _outputScanDataFile_self(_out_file, convertData, vldLineNum, + lineV, maxTimeStamp, clockPerSecond); + printf("%s: convert done!\n", _Guangzi_file); +#endif #if TEST_CONVERT_TO_GRID //将数据转换成栅格格式格式 char _scan_dir[256]; @@ -1111,7 +1267,7 @@ int main() sg_lineDataR(&laser3DPoints[i], poseCalibPara.planeCalib, poseCalibPara.planeHeight); } //计算节点位置 - std::vector nodePos; + std::vector> nodePos; sg_getSieveNodes( laser3DPoints, lineNum, diff --git a/bagPositioning_test/bagPositioning_test.cpp b/bagPositioning_test/bagPositioning_test.cpp index 89ed7a7..5c77ba2 100644 --- a/bagPositioning_test/bagPositioning_test.cpp +++ b/bagPositioning_test/bagPositioning_test.cpp @@ -11,6 +11,17 @@ #include #include +typedef struct +{ + int nPointIdx; + double x; + double y; + double z; + float r; + float g; + float b; +} SPointXYZRGB; + SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, double matrix3d[9]) { SVzNL3DPoint _r_pt; @@ -18,7 +29,15 @@ SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, double matrix3d[9]) _r_pt.y = pt3D.x * matrix3d[3] + pt3D.y * matrix3d[4] + pt3D.z * matrix3d[5]; _r_pt.z = pt3D.x * matrix3d[6] + pt3D.y * matrix3d[7] + pt3D.z * matrix3d[8]; return _r_pt; +} +SVzNLPointXYZRGBA _ptRotate_RGBD(SVzNLPointXYZRGBA pt3D, double matrix3d[9]) +{ + SVzNLPointXYZRGBA _r_pt; + _r_pt.x = pt3D.x * matrix3d[0] + pt3D.y * matrix3d[1] + pt3D.z * matrix3d[2]; + _r_pt.y = pt3D.x * matrix3d[3] + pt3D.y * matrix3d[4] + pt3D.z * matrix3d[5]; + _r_pt.z = pt3D.x * matrix3d[6] + pt3D.y * matrix3d[7] + pt3D.z * matrix3d[8]; + return _r_pt; } #define DATA_VER_OLD 0 @@ -134,15 +153,24 @@ SVzNLXYZRGBDLaserLine* vzReadLaserScanPointFromFile_XYZRGB(const char* fileName, int imageY = 0; float leftX, leftY; float rightX, rightY; - int r = -1, g = -1, b = -1; + float r, g, b; sscanf_s(linedata.c_str(), "{%f,%f,%f,%f,%f,%f }-{%f,%f}-{%f,%f}", &X, &Y, &Z, &r, &g, &b, &leftX, &leftY, &rightX, &rightY); int id = _scanLines[lineIdx].nPointCnt; if (id < ptNum) { + if (lineIdx == 537) + int kkk = 1; p3DPoint[id].x = X; p3DPoint[id].y = Y; p3DPoint[id].z = Z; - p3DPoint[id].nRGB = 0; + int nr = (int)(r * 255); + int ng = (int)(g * 255); + int nb = (int)(b * 255); + nb <<= 8; + nb += ng; + nb <<= 8; + nb += nr; + p3DPoint[id].nRGB = nb; _scanLines[lineIdx].nPointCnt = id + 1; } } @@ -209,6 +237,41 @@ SVzNLXYZRGBDLaserLine* vzReadLaserScanPointFromFile_XYZRGB(const char* fileName, return _scanLines; } +void vzReadLaserScanPointFromFile_XYZRGB_vector(const char* fileName, std::vector>& scanData) +{ + std::ifstream inputFile(fileName); + std::string linedata; + + if (inputFile.is_open() == false) + return; + + std::vector< SPointXYZRGB> a_line; + while (getline(inputFile, linedata)) + { + if (0 == strncmp("Line_", linedata.c_str(), 5)) //new line + { + if (a_line.size() > 0) + { + scanData.push_back(a_line); + a_line.clear(); + } + } + else if (0 == strncmp("{", linedata.c_str(), 1)) + { + float leftX, leftY; + float rightX, rightY; + SPointXYZRGB a_pt; + sscanf_s(linedata.c_str(), "{ %lf, %lf, %lf, %f, %f, %f }-{ %f, %f }-{ %f, %f }", &a_pt.x, &a_pt.y, &a_pt.z, &a_pt.r, &a_pt.g, &a_pt.b, &leftX, &leftY, &rightX, &rightY); + a_line.push_back(a_pt); + } + } + if(a_line.size() > 0) + scanData.push_back(a_line); + + inputFile.close(); + return; +} + SVzNL3DLaserLine* vzReadLaserScanPointFromFile_XYZ(const char* fileName, int* scanLineNum, float* scanV, int* dataCalib, int* scanMaxStamp, int* canClockUnit) { @@ -453,6 +516,53 @@ SVzNL3DLaserLine* _convertToGridData_XYZRGB(SVzNLXYZRGBDLaserLine* laser3DPoints return gridData; } +void _convertToGridData_XYZRGB_vector(std::vector>& scanData, double _F, std::vector>& scanData_grid) +{ + int min_y = 100000000; + int max_y = -10000000; + int lineNum = scanData.size(); + for (int line = 0; line < lineNum; line++) + { + std::vector< SPointXYZRGB>& a_line = scanData[line]; + int nPointCnt = a_line.size(); + for (int i = 0; i < nPointCnt; i++) + { + SPointXYZRGB* a_pt = &scanData[line][i]; + if (a_pt->z > 1e-4) + { + double v = _F * a_pt->y / a_pt->z + 2000; + a_pt->nPointIdx = (int)(v + 0.5); + max_y = max_y < (int)a_pt->nPointIdx ? (int)a_pt->nPointIdx : max_y; + min_y = min_y > (int)a_pt->nPointIdx ? (int)a_pt->nPointIdx : min_y; + } + } + } + if (min_y == 100000000) + return; + + int pt_counter = max_y - min_y + 1; + std::vector< SPointXYZRGB> gridData; + gridData.resize(pt_counter); + for (int i = 0; i < pt_counter; i++) + gridData[i] = { 0, 0.0, 0.0, 0.0, 0.0f, 0.0f, 0.0f }; + for (int line = 0; line < lineNum; line++) + { + std::vector< SPointXYZRGB>& a_line = scanData[line]; + int nPointCnt = a_line.size(); + for (int i = 0; i < nPointCnt; i++) + { + SPointXYZRGB a_pt = a_line[i]; + if (a_pt.z > 1e-4) + { + int pt_id = a_pt.nPointIdx - min_y; + gridData[pt_id]= a_pt; + } + } + scanData_grid.push_back(gridData); + } + return; +} + void _outputCalibPara(char* fileName, SSG_planeCalibPara calibPara) { std::ofstream sw(fileName); @@ -546,6 +656,36 @@ void _outputScanDataFile_self(char* fileName, SVzNL3DLaserLine* scanData, int li sw.close(); } +void _outputScanDataFile_SPointXYZRGB_vector(char* fileName, std::vector>& scanData) +{ + std::ofstream sw(fileName); + int lineNum = scanData.size(); + sw << "LineNum:" << lineNum << std::endl; + sw << "DataType: 0" << std::endl; + sw << "ScanSpeed: 0" << std::endl; + sw << "PointAdjust: 1" << std::endl; + sw << "MaxTimeStamp: 0_0" << std::endl; + + for (int line = 0; line < lineNum; line++) + { + int nPositionCnt = scanData[line].size(); + sw << "Line_" << line << "_0_" << nPositionCnt << std::endl; + for (int i = 0; i < nPositionCnt; i++) + { + SPointXYZRGB* pt3D = &scanData[line][i]; + float x = (float)pt3D->x; + float y = (float)pt3D->y; + float z = (float)pt3D->z; + char str[250]; + sprintf_s(str, "{ %lf, %lf, %lf, %f, %f, %f } - { 0, 0 } - { 0, 0 }", + pt3D->x, pt3D->y, pt3D->z, pt3D->r, pt3D->g, pt3D->b); + + sw << str << std::endl; + } + } + sw.close(); +} + typedef struct { int r; @@ -704,6 +844,307 @@ void _outputScanDataFile_RGBD_obj(char* fileName, SVzNL3DLaserLine* scanData, in sw.close(); } +void _outputScanDataFile_obj_vector(char* fileName, std::vector>scanData, + std::vector& objOps) +{ + int lineNum = scanData.size(); + std::ofstream sw(fileName); + int realLines = lineNum; + if (objOps.size() > 0) + realLines++; + sw << "LineNum:" << realLines << std::endl; + sw << "DataType: 0" << std::endl; + sw << "ScanSpeed: 0" << std::endl; + sw << "PointAdjust: 1" << std::endl; + sw << "MaxTimeStamp: 0_0" << std::endl; + + int maxLineIndex = 0; + int max_stamp = 0; + + SG_color rgb = { 0, 0, 0 }; + + SG_color objColor[8] = { + {245,222,179},//淡黄色 + {210,105, 30},//巧克力色 + {240,230,140},//黄褐色 + {135,206,235},//天蓝色 + {250,235,215},//古董白 + {189,252,201},//薄荷色 + {221,160,221},//梅红色 + {188,143,143},//玫瑰红色 + }; + int size = 1; + int nTimeStamp = 0; + for (int line = 0; line < lineNum; line++) + { + int nPositionCnt = scanData[line].size(); + sw << "Line_" << line << "_0_" << nPositionCnt << std::endl; + for (int i = 0; i < nPositionCnt; i++) + { + SVzNL3DPosition* pt3D = &scanData[line][i]; + int vType = pt3D->nPointIdx & 0xff; + int hType = vType >> 4; + int objId = (pt3D->nPointIdx >> 16) & 0xffff; + vType = vType & 0x0f; + if (LINE_FEATURE_L_JUMP_H2L == vType) + { + rgb = { 255, 97, 0 }; + size = 3; + } + else if (LINE_FEATURE_L_JUMP_L2H == vType) + { + rgb = { 255, 255, 0 }; + size = 3; + } + else if (LINE_FEATURE_V_SLOPE == vType) + { + rgb = { 255, 0, 255 }; + size = 3; + } + else if (LINE_FEATURE_L_SLOPE_H2L == vType) + { + rgb = { 160, 82, 45 }; + size = 3; + } + else if ((LINE_FEATURE_LINE_ENDING_0 == vType) || (LINE_FEATURE_LINE_ENDING_1 == vType)) + { + rgb = { 255, 0, 0 }; + size = 3; + } + else if (LINE_FEATURE_L_SLOPE_L2H == vType) + { + rgb = { 233, 150, 122 }; + size = 3; + } + else if (LINE_FEATURE_L_JUMP_H2L == hType) + { + rgb = { 0, 0, 255 }; + size = 3; + } + else if (LINE_FEATURE_L_JUMP_L2H == hType) + { + rgb = { 0, 255, 255 }; + size = 3; + } + else if (LINE_FEATURE_V_SLOPE == hType) + { + rgb = { 0, 255, 0 }; + size = 3; + } + else if (LINE_FEATURE_L_SLOPE_H2L == hType) + { + rgb = { 85, 107, 47 }; + size = 3; + } + else if (LINE_FEATURE_L_SLOPE_L2H == hType) + { + rgb = { 0, 255, 154 }; + size = 3; + } + else if ((LINE_FEATURE_LINE_ENDING_0 == hType) || (LINE_FEATURE_LINE_ENDING_1 == hType)) + { + rgb = { 255, 0, 0 }; + size = 3; + } + else if (objId > 0) //目标 + { + rgb = objColor[objId % 8]; + size = 5; + } + else + { + rgb = { 200, 200, 200 }; + size = 1; + } + + float x = (float)pt3D->pt3D.x; + float y = (float)pt3D->pt3D.y; + float z = (float)pt3D->pt3D.z; + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; + } + } + if (objOps.size() > 0) + { + int ptNum = objOps.size(); + sw << "Line_" << lineNum << "_0_" << ptNum << std::endl; + for (int i = 0; i < objOps.size(); i++) + { + if (i == 0) + rgb = { 255, 0, 0 }; + else + rgb = { 255, 255, 0 }; + size = 25; + float x = (float)objOps[i].centerPos.x; + float y = (float)objOps[i].centerPos.y; + float z = (float)objOps[i].centerPos.z; + + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; + if (i == 0) + { + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; + } + } + } + //画出方向线 + sw.close(); +} + +void _outputRGBDScanDataFile_RGBD_obj(char* fileName, SVzNLXYZRGBDLaserLine* scanData, int lineNum, + float lineV, int maxTimeStamp, int clockPerSecond, std::vector& objOps) +{ + std::ofstream sw(fileName); + int realLines = lineNum; + if (objOps.size() > 0) + realLines++; + sw << "LineNum:" << realLines << std::endl; + sw << "DataType: 0" << std::endl; + sw << "ScanSpeed:" << lineV << std::endl; + sw << "PointAdjust: 1" << std::endl; + sw << "MaxTimeStamp:" << maxTimeStamp << "_" << clockPerSecond << std::endl; + + int maxLineIndex = 0; + int max_stamp = 0; + + SG_color rgb = { 0, 0, 0 }; + + SG_color objColor[8] = { + {245,222,179},//淡黄色 + {210,105, 30},//巧克力色 + {240,230,140},//黄褐色 + {135,206,235},//天蓝色 + {250,235,215},//古董白 + {189,252,201},//薄荷色 + {221,160,221},//梅红色 + {188,143,143},//玫瑰红色 + }; + int size = 1; + int nTimeStamp = 0; + for (int line = 0; line < lineNum; line++) + { + sw << "Line_" << line << "_" << scanData[line].nTimeStamp << "_" << scanData[line].nPointCnt << std::endl; + nTimeStamp = scanData[line].nTimeStamp; + for (int i = 0; i < scanData[line].nPointCnt; i++) + { + SVzNLPointXYZRGBA* pt3D = &scanData[line].p3DPoint[i]; +#if 0 + int vType = pt3D->nRGB & 0xff; + int hType = vType >> 4; + int objId = (pt3D->nRGB >> 16) & 0xffff; + vType = vType & 0x0f; + if (LINE_FEATURE_L_JUMP_H2L == vType) + { + rgb = { 255, 97, 0 }; + size = 3; + } + else if (LINE_FEATURE_L_JUMP_L2H == vType) + { + rgb = { 255, 255, 0 }; + size = 3; + } + else if (LINE_FEATURE_V_SLOPE == vType) + { + rgb = { 255, 0, 255 }; + size = 3; + } + else if (LINE_FEATURE_L_SLOPE_H2L == vType) + { + rgb = { 160, 82, 45 }; + size = 3; + } + else if ((LINE_FEATURE_LINE_ENDING_0 == vType) || (LINE_FEATURE_LINE_ENDING_1 == vType)) + { + rgb = { 255, 0, 0 }; + size = 3; + } + else if (LINE_FEATURE_L_SLOPE_L2H == vType) + { + rgb = { 233, 150, 122 }; + size = 3; + } + else if (LINE_FEATURE_L_JUMP_H2L == hType) + { + rgb = { 0, 0, 255 }; + size = 3; + } + else if (LINE_FEATURE_L_JUMP_L2H == hType) + { + rgb = { 0, 255, 255 }; + size = 3; + } + else if (LINE_FEATURE_V_SLOPE == hType) + { + rgb = { 0, 255, 0 }; + size = 3; + } + else if (LINE_FEATURE_L_SLOPE_H2L == hType) + { + rgb = { 85, 107, 47 }; + size = 3; + } + else if (LINE_FEATURE_L_SLOPE_L2H == hType) + { + rgb = { 0, 255, 154 }; + size = 3; + } + else if ((LINE_FEATURE_LINE_ENDING_0 == hType) || (LINE_FEATURE_LINE_ENDING_1 == hType)) + { + rgb = { 255, 0, 0 }; + size = 3; + } + else if (objId > 0) //目标 + { + rgb = objColor[objId % 8]; + size = 5; + } + else +#endif + { + rgb = { 200, 200, 200 }; + size = 1; + } + float x = (float)pt3D->x; + float y = (float)pt3D->y; + float z = (float)pt3D->z; + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; + } + } + if (objOps.size() > 0) + { + int ptNum = objOps.size(); + sw << "Line_" << lineNum << "_" << (nTimeStamp + 1000) << "_" << ptNum << std::endl; + for (int i = 0; i < objOps.size(); i++) + { + if (i == 0) + rgb = { 255, 0, 0 }; + else + rgb = { 255, 255, 0 }; + size = 25; + float x = (float)objOps[i].centerPos.x; + float y = (float)objOps[i].centerPos.y; + float z = (float)objOps[i].centerPos.z; + + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; + if (i == 0) + { + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; + } + } + } + sw.close(); +} + void _outputScanDataFile_RGBD_sideBagObj(char* fileName, SVzNL3DLaserLine* scanData, int lineNum, float lineV, int maxTimeStamp, int clockPerSecond, std::vector& objOps) { @@ -931,8 +1372,63 @@ void _rotateCloudPts(SVzNL3DLaserLine* scanData, int lineNum, double matrix3d[9] } } -void _XOYprojection(cv::Mat& img, std::vector>& dataLines, std::vector& objOps, - const double x_scale, const double y_scale, const SVzNLRangeD x_range, const SVzNLRangeD y_range, bool drawDirAngle) +void _rotateCloudPts_RGBD(SVzNLXYZRGBDLaserLine* scanData, int lineNum, double matrix3d[9], std::vector>& rotateLines, SVzNLRangeD* rx_range, SVzNLRangeD* ry_range) +{ + rx_range->min = 0; + rx_range->max = -1; + ry_range->min = 0; + ry_range->max = -1; + for (int line = 0; line < lineNum; line++) + { + std::vector< SVzNLPointXYZRGBA> linePts; + for (int i = 0; i < scanData[line].nPointCnt; i++) + { + SVzNLPointXYZRGBA* pt3D = &scanData[line].p3DPoint[i]; + if (pt3D->z < 1e-4) + continue; + + SVzNLPointXYZRGBA r_pt; + r_pt = _ptRotate_RGBD(*pt3D, matrix3d); + if (rx_range->max < rx_range->min) + { + rx_range->min = r_pt.x; + rx_range->max = r_pt.x; + } + else + { + if (rx_range->min > r_pt.x) + rx_range->min = r_pt.x; + if (rx_range->max < r_pt.x) + rx_range->max = r_pt.x; + } + if (ry_range->max < ry_range->min) + { + ry_range->min = r_pt.y; + ry_range->max = r_pt.y; + } + else + { + if (ry_range->min > r_pt.y) + ry_range->min = r_pt.y; + if (ry_range->max < r_pt.y) + ry_range->max = r_pt.y; + } + linePts.push_back(r_pt); + } + rotateLines.push_back(linePts); + } +} + +void _XOYprojection( + cv::Mat& img, + std::vector>& dataLines, + std::vector& objOps, + const double x_scale, + const double y_scale, + const SVzNLRangeD x_range, + const SVzNLRangeD y_range, + bool drawDirAngle, + int dirAngleLen) { int x_skip = 16; int y_skip = 16; @@ -1029,7 +1525,7 @@ void _XOYprojection(cv::Mat& img, std::vector>& da } else { - rgb = { 60, 60, 60 }; + rgb = { 150, 150, 150 }; size = 1; } @@ -1065,7 +1561,7 @@ void _XOYprojection(cv::Mat& img, std::vector>& da if (true == drawDirAngle) { //画线 - double R = 100; + double R = (double)dirAngleLen / 2.0; const double deg2rad = PI / 180.0; const double yaw = objOps[i].centerPos.z_yaw * deg2rad; double cy = cos(yaw); double sy = sin(yaw); @@ -1082,6 +1578,109 @@ void _XOYprojection(cv::Mat& img, std::vector>& da } +void _XOYprojection_RGBD( + cv::Mat& img, + std::vector>& dataLines, + std::vector& objOps, + const double x_scale, + const double y_scale, + const SVzNLRangeD x_range, + const SVzNLRangeD y_range, + bool drawDirAngle, + int dirAngleLen) +{ + int x_skip = 16; + int y_skip = 16; + + cv::Vec3b rgb = cv::Vec3b(0, 0, 0); + cv::Vec3b objColor[8] = { + {245,222,179},//淡黄色 + {210,105, 30},//巧克力色 + {240,230,140},//黄褐色 + {135,206,235},//天蓝色 + {250,235,215},//古董白 + {189,252,201},//薄荷色 + {221,160,221},//梅红色 + {188,143,143},//玫瑰红色 + }; + int size = 1; + for (int line = 0; line < dataLines.size(); line++) + { + std::vector< SVzNLPointXYZRGBA>& a_line = dataLines[line]; + for (int i = 0; i < a_line.size(); i++) + { + SVzNLPointXYZRGBA* pt3D = &a_line[i]; + if (pt3D->z < 1e-4) + continue; + + int nRGB = pt3D->nRGB; + int r = nRGB & 0xff; + nRGB >>= 8; + int g = nRGB & 0xff; + nRGB >>= 8; + int b = nRGB & 0xff; + + rgb[0] = r; + rgb[1] = g; + rgb[2] = b; + size = 1; + double x = pt3D->x; + double y = pt3D->y; + int px = (int)((x - x_range.min) / x_scale + x_skip); + int py = (int)((y - y_range.min) / y_scale + y_skip); + if ((px == 666) && (py == 828)) + int kkk = 1; + if (size == 1) + img.at(py, px) = cv::Vec3b(rgb[2], rgb[1], rgb[0]); + else + cv::circle(img, cv::Point(px, py), size, cv::Scalar(rgb[2], rgb[1], rgb[0]), -1); + } + } + if (objOps.size() > 0) + { + for (int i = 0; i < objOps.size(); i++) + { + if (i == 0) + { + rgb = { 255, 0, 0 }; + size = 20; + } + else + { + rgb = { 255, 255, 0 }; + size = 10; + } + int px = (int)((objOps[i].centerPos.x - x_range.min) / x_scale + x_skip); + int py = (int)((objOps[i].centerPos.y - y_range.min) / y_scale + y_skip); + cv::circle(img, cv::Point(px, py), size, cv::Scalar(rgb[2], rgb[1], rgb[0]), -1); + if (true == drawDirAngle) + { + //画线 + double R = (double)dirAngleLen / 2.0; + const double deg2rad = PI / 180.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 - x_range.min) / x_scale + x_skip); + int py1 = (int)((y1 - y_range.min) / y_scale + y_skip); + int px2 = (int)((x2 - x_range.min) / x_scale + x_skip); + int py2 = (int)((y2 - y_range.min) / y_scale + y_skip); + cv::line(img, cv::Point(px1, py1), cv::Point(px2, py2), cv::Scalar(rgb[2], rgb[1], rgb[0]), 2); + if (objOps[i].orienFlag == 1) + { + cv::circle(img, cv::Point(px1, py1), 5, cv::Scalar(0, 255, 0), -1); + } + else if (objOps[i].orienFlag == 2) + { + cv::circle(img, cv::Point(px1, py1), 5, cv::Scalar(0, 0, 255), -1); + } + } + } + } + +} + void _XOYprojection_sideBagInfo(cv::Mat& img, std::vector>& dataLines, std::vector& objOps, const double x_scale, const double y_scale, const SVzNLRangeD x_range, const SVzNLRangeD y_range) { @@ -1250,7 +1849,7 @@ void _XOYprojection_sideBagInfo(cv::Mat& img, std::vector& objOps, double rpy[3]) +void _genXOYProjectionImage(cv::String& fileName, SVzNL3DLaserLine* scanData, int lineNum, std::vector& objOps, double rpy[3], double dirLen) { //统计X和Y的范围 std::vector> scan_lines; @@ -1302,7 +1901,13 @@ void _genXOYProjectionImage(cv::String& fileName, SVzNL3DLaserLine* scanData, in //计算投影比例 double x_scale = (x_range.max - x_range.min) / x_cols; double y_scale = (y_range.max - y_range.min) / y_rows; - _XOYprojection(img, scan_lines, objOps, x_scale, y_scale, x_range, y_range, true); + if (x_scale < y_scale) + x_scale = y_scale; + else + y_scale = x_scale; + + int angleDrawLen = dirLen / x_scale; + _XOYprojection(img, scan_lines, objOps, x_scale, y_scale, x_range, y_range, true, angleDrawLen); //旋转视角显示 double matrix3d[9]; @@ -1324,7 +1929,188 @@ void _genXOYProjectionImage(cv::String& fileName, SVzNL3DLaserLine* scanData, in //计算投影比例 double rx_scale = (rx_range.max - rx_range.min) / x_cols; double ry_scale = (ry_range.max - ry_range.min) / y_rows; - _XOYprojection(r_img, rotateLines, r_objOps, rx_scale, ry_scale, rx_range, ry_range, false); + _XOYprojection(r_img, rotateLines, r_objOps, rx_scale, ry_scale, rx_range, ry_range, false, angleDrawLen); + + cv::Mat dis_img; + cv::hconcat(img, r_img, dis_img); + cv::imwrite(fileName, dis_img); + return; +} + +void _RGBDto2DImage(char* fileName, SVzNLXYZRGBDLaserLine* scanData, int lineNum) +{ + cv::String imgName(fileName); + //统计X和Y的范围 + SVzNLRangeD x_range = { 0, -1 }; + SVzNLRangeD y_range = { 0, -1 }; + 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; + + if (x_range.max < x_range.min) + { + x_range.min = pt3D->x; + x_range.max = pt3D->x; + } + else + { + if (x_range.min > pt3D->x) + x_range.min = pt3D->x; + if (x_range.max < pt3D->x) + x_range.max = pt3D->x; + } + if (y_range.max < y_range.min) + { + y_range.min = pt3D->y; + y_range.max = pt3D->y; + } + else + { + if (y_range.min > pt3D->y) + y_range.min = pt3D->y; + if (y_range.max < pt3D->y) + y_range.max = pt3D->y; + } + } + } + + int x_skip = 16; + int y_skip = 16; + int imgRows = 992; + int imgCols = 1056; + double y_rows = (double)(imgRows - y_skip *2); + double x_cols = (double)(imgCols - x_skip *2); + cv::Mat img = cv::Mat::zeros(imgRows, imgCols, CV_8UC3); + //计算投影比例 + double x_scale = (x_range.max - x_range.min) / x_cols; + double y_scale = (y_range.max - y_range.min) / y_rows; + if (x_scale < y_scale) + x_scale = y_scale; + else + y_scale = x_scale; + + int size = 1; + cv::Vec3b rgb = cv::Vec3b(0, 0, 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; + + int nRGB = pt3D->nRGB; + int r = nRGB & 0xff; + nRGB >>= 8; + int g = nRGB & 0xff; + nRGB >>= 8; + int b = nRGB & 0xff; + + rgb[0] = r; + rgb[1] = g; + rgb[2] = b; + size = 1; + double x = pt3D->x; + double y = pt3D->y; + int px = (int)((x - x_range.min) / x_scale + x_skip); + int py = (int)((y - y_range.min) / y_scale + y_skip); + if ((px == 666) && (py == 828)) + int kkk = 1; + if (size == 1) + img.at(py, px) = cv::Vec3b(rgb[2], rgb[1], rgb[0]); + else + cv::circle(img, cv::Point(px, py), size, cv::Scalar(rgb[2], rgb[1], rgb[0]), -1); + } + } + cv::imwrite(imgName, img); + return; +} + +void _genXOYProjectionImage_RGBD(cv::String& fileName, SVzNLXYZRGBDLaserLine* scanData, int lineNum, std::vector& objOps, double rpy[3], double dirLen) +{ + //统计X和Y的范围 + std::vector> scan_lines; + SVzNLRangeD x_range = { 0, -1 }; + SVzNLRangeD y_range = { 0, -1 }; + for (int line = 0; line < lineNum; line++) + { + std::vector< SVzNLPointXYZRGBA> a_line; + for (int i = 0; i < scanData[line].nPointCnt; i++) + { + SVzNLPointXYZRGBA* pt3D = &scanData[line].p3DPoint[i]; + if (pt3D->z < 1e-4) + continue; + + a_line.push_back(*pt3D); + if (x_range.max < x_range.min) + { + x_range.min = pt3D->x; + x_range.max = pt3D->x; + } + else + { + if (x_range.min > pt3D->x) + x_range.min = pt3D->x; + if (x_range.max < pt3D->x) + x_range.max = pt3D->x; + } + if (y_range.max < y_range.min) + { + y_range.min = pt3D->y; + y_range.max = pt3D->y; + } + else + { + if (y_range.min > pt3D->y) + y_range.min = pt3D->y; + if (y_range.max < pt3D->y) + y_range.max = pt3D->y; + } + } + scan_lines.push_back(a_line); + } + + int imgRows = 992; + int imgCols = 1056; + double y_rows = 960.0; + double x_cols = 1024.0; + cv::Mat img = cv::Mat::zeros(imgRows, imgCols, CV_8UC3); + //计算投影比例 + double x_scale = (x_range.max - x_range.min) / x_cols; + double y_scale = (y_range.max - y_range.min) / y_rows; + if (x_scale < y_scale) + x_scale = y_scale; + else + y_scale = x_scale; + + int angleDrawLen = dirLen / x_scale; + _XOYprojection_RGBD(img, scan_lines, objOps, x_scale, y_scale, x_range, y_range, true, angleDrawLen); + + //旋转视角显示 + double matrix3d[9]; + EulerRpyToRotation1(rpy, matrix3d); + std::vector r_objOps; + r_objOps.insert(r_objOps.end(), objOps.begin(), objOps.end()); + for (int i = 0; i < objOps.size(); i++) + { + SVzNL3DPoint c_pt = { objOps[i].centerPos.x, objOps[i].centerPos.y, objOps[i].centerPos.z }; + SVzNL3DPoint r_c_pt = _ptRotate(c_pt, matrix3d); + r_objOps[i].centerPos.x = r_c_pt.x; + r_objOps[i].centerPos.y = r_c_pt.y; + r_objOps[i].centerPos.z = r_c_pt.z; + } + std::vector> rotateLines; + SVzNLRangeD rx_range, ry_range; + _rotateCloudPts_RGBD(scanData, lineNum, matrix3d, rotateLines, &rx_range, &ry_range); + cv::Mat r_img = cv::Mat::zeros(imgRows, imgCols, CV_8UC3); + //计算投影比例 + double rx_scale = (rx_range.max - rx_range.min) / x_cols; + double ry_scale = (ry_range.max - ry_range.min) / y_rows; + _XOYprojection_RGBD(r_img, rotateLines, r_objOps, rx_scale, ry_scale, rx_range, ry_range, false, angleDrawLen); cv::Mat dis_img; cv::hconcat(img, r_img, dis_img); @@ -1614,11 +2400,36 @@ void _outputScanDataFile_removeZeros(char* fileName, SVzNL3DLaserLine* scanData, } #define TEST_CONVERT_TO_GRID 0 +#define TEST_CONVERT_RGBD_TO_GRID 0 #define TEST_COMPUTE_GRASP_POINT 1 -#define TEST_COMPUTE_CALIB_PARA 1 -#define TEST_GROUP 16 +#define TEST_COMPUTE_CALIB_PARA 0 + +#define TEST_GROUP 21 +#define TEST_TOP_VIEW_GROUP (TEST_GROUP - 8) +#define TEST_TOP_ORIEN_GROUP (TEST_GROUP - 6) int main() { +#if TEST_CONVERT_RGBD_TO_GRID + char _scan_dir[256]; + sprintf_s(_scan_dir, "F:\\ShangGu\\编织袋数据\\编织袋RGBD识别方向_2\\"); + char _in_file[256]; + double _F = 1247.95;// 1231.2; //1729.0;; //f + for (int i = 1; i <= 28; i++) + { + sprintf_s(_in_file, "%sLaserLine%d.txt", _scan_dir, i); + std::vector> scanData; + vzReadLaserScanPointFromFile_XYZRGB_vector(_in_file, scanData); + if (scanData.size() == 0) + continue; + + std::vector> gridData; + _convertToGridData_XYZRGB_vector(scanData, _F, gridData); + char _out_file[256]; + sprintf_s(_out_file, "%s%d-RGB点云.txt", _scan_dir, i); + _outputScanDataFile_SPointXYZRGB_vector(_out_file, gridData); + printf("%s: convert done!\n", _in_file); + } +#endif #if TEST_CONVERT_TO_GRID //将数据转换成栅格格式格式 char _scan_dir[256]; @@ -1627,17 +2438,17 @@ int main() int dataCalib = 0; int maxTimeStamp = 0; int clockPerSecond = 0; - sprintf_s(_scan_dir, "F:\\上古\\编织袋数据\\侧抓数据\\"); + sprintf_s(_scan_dir, "F:\\ShangGu\\编织袋数据\\编织袋RGBD识别方向_2\\"); char _scan_file[256]; - double _F = 1248.5;// 1231.2; //1729.0;; //f - for (int i = 1; i <= 2; i++) + double _F = 1247.95;// 1231.2; //1729.0;; //f + for (int i = 1; i <= 28; i++) { - sprintf_s(_scan_file, "%s%d_LaserData_Hi228621.txt", _scan_dir, i); + sprintf_s(_scan_file, "%sLaserLine%d.txt", _scan_dir, i); SVzNLXYZRGBDLaserLine* laser3DPoints_RGBD = vzReadLaserScanPointFromFile_XYZRGB(_scan_file, &lineNum, &lineV, &dataCalib, &maxTimeStamp, &clockPerSecond); if (laser3DPoints_RGBD == NULL) continue; -#if 1 +#if 0 double rpy[3] = { 0,-18, 0 }; double camPoseR[9]; EulerRpyToRotation1(rpy, camPoseR); @@ -1682,7 +2493,7 @@ int main() #if TEST_COMPUTE_CALIB_PARA char _calib_datafile[256]; - sprintf_s(_calib_datafile, "F:\\上古\\编织袋数据\\点云8_广东1213-1215数据\\LaserLine10_grid.txt"); + sprintf_s(_calib_datafile, "F:\\ShangGu\\编织袋数据\\编织袋RGBD识别方向\\底面点云\\底面点云.txt"); int lineNum = 0; float lineV = 0.0f; int dataCalib = 0; @@ -1701,14 +2512,14 @@ int main() int kkk = 1; //行处理 //调平，去除地面 - sg_lineDataR(&laser3DPoints[i], calibPara.planeCalib, calibPara.planeHeight); + sg_lineDataR(&laser3DPoints[i], calibPara.planeCalib, -1);// calibPara.planeHeight); } // char calibFile[250]; - sprintf_s(calibFile, "F:\\上古\\编织袋数据\\点云8_广东1213-1215数据\\ground_calib_para.txt"); + sprintf_s(calibFile, "F:\\ShangGu\\编织袋数据\\编织袋RGBD识别方向\\ground_calib_para.txt"); _outputCalibPara(calibFile, calibPara); char _out_file[256]; - sprintf_s(_out_file, "F:\\上古\\编织袋数据\\点云8_广东1213-1215数据\\LaserLine10_grid_calib.txt"); + sprintf_s(_out_file, "F:\\ShangGu\\编织袋数据\\编织袋RGBD识别方向\\LaserLine1_grid_calib.txt"); _outputScanDataFile_self(_out_file, laser3DPoints, lineNum, lineV, maxTimeStamp, clockPerSecond); printf("%s: calib done!\n", _calib_datafile); @@ -1728,17 +2539,23 @@ int main() "F:\\ShangGu\\编织袋数据\\点云8_广东1213-1215数据\\", //7 "F:\\ShangGu\\编织袋数据\\点云9_广东1219数据\\", //8 "F:\\ShangGu\\编织袋数据\\点云10\\", //9 - "F:\\ShangGu\\编织袋数据\\侧抓数据\\", //10 - "F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250419\\", //11 - "F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250420-1\\", //12 - "F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250420-2\\", //13 - "F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250420-3\\", //14 - "F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250420-4\\", //15 + "F:\\ShangGu\\编织袋数据\\点云11_盐袋\\", //10 + "F:\\ShangGu\\编织袋数据\\点云12_盐袋\\", //11 + "F:\\ShangGu\\编织袋数据\\点云13_现场测试\\", //12 + "F:\\ShangGu\\编织袋数据\\编织袋RGBD识别方向\\", //13 + "F:\\ShangGu\\编织袋数据\\编织袋RGBD识别方向_2\\", //14 + "F:\\ShangGu\\编织袋数据\\侧抓数据\\", //15 + "F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250419\\", //16 + "F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250420-1\\", //17 + "F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250420-2\\", //18 + "F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250420-3\\", //19 + "F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250420-4\\", //20 }; SVzNLRange fileIdx[TEST_GROUP] = { {0,176},{1,200},{1,166},{122,141},{1,65}, {1,29},{108,135},{0,200}, {1,200}, {1,12}, + {2,4}, {1,5}, {1,1}, {1,21},{1,28}, {3,3}, {1,51}, {4,83}, {1,74}, {1,61}, {1,84} }; @@ -1758,58 +2575,126 @@ int main() for (int i = 0; i < 9; i++) poseCalibPara.invRMatrix[i] = poseCalibPara.planeCalib[i]; - SG_bagPositionParam algoParam; - algoParam.bagParam.bagL = 650; //袋子长65cm - algoParam.bagParam.bagW = 450; //袋子宽40cm - algoParam.bagParam.bagH = 160; //袋子高16cm - -#if BAG_ALGO_USE_CORNER_FEATURE - algoParam.cornerParam.cornerTh = 30; //45度角 - algoParam.cornerParam.scale = 15; // 30; // algoParam.bagParam.bagH / 8; - algoParam.cornerParam.minEndingGap = algoParam.bagParam.bagW / 4; - algoParam.cornerParam.jumpCornerTh_1 = 60; - algoParam.cornerParam.jumpCornerTh_2 = 15; -#else - algoParam.slopeParam.LSlopeZWin = 10.0; - algoParam.slopeParam.validSlopeH = 10.0; - algoParam.slopeParam.minLJumpH = 20.0; - algoParam.slopeParam.minEndingGap = algoParam.bagParam.bagW / 4; ///当没有中间袋子，只有左右袋子时，有两组Ending。此参数确定两个ending最小间距 - algoParam.valleyPara.valleyMinH = 10.0; - algoParam.valleyPara.valleyMaxW = 80.0; //取袋子宽度的1/5 -#endif - algoParam.growParam.maxLineSkipNum = 5; - algoParam.growParam.yDeviation_max = 20.0; - algoParam.growParam.maxSkipDistance = 20.0; - algoParam.growParam.zDeviation_max = 80.0; //袋子高度1/2 - algoParam.growParam.minLTypeTreeLen = 50.0; //mm - algoParam.growParam.minVTypeTreeLen = 50.0; //mm - char _scan_file[256]; + SG_bagPositionParam algoParam; int endGroup = TEST_GROUP - 1; - for (int grp = 0; grp <= 9; grp++) + for (int grp = 14; grp <= 14; grp++) { + if (grp < 10) + { + algoParam.bagParam.bagL = 650; //袋子长65cm + algoParam.bagParam.bagW = 450; //袋子宽40cm + algoParam.bagParam.bagH = 160; //袋子高16cm + algoParam.growParam.maxLineSkipNum = 5; + algoParam.growParam.yDeviation_max = 20.0; + algoParam.growParam.maxSkipDistance = 20.0; + algoParam.growParam.zDeviation_max = algoParam.bagParam.bagH / 2; //袋子高度1/2 + algoParam.growParam.minLTypeTreeLen = 50.0; //mm + algoParam.growParam.minVTypeTreeLen = 50.0; //mm + } + else if ( (grp >= 10) && (grp <= 11)) + { + algoParam.bagParam.bagL = 150; //袋子长65cm + algoParam.bagParam.bagW = 110; //袋子宽40cm + algoParam.bagParam.bagH = 20; //袋子高16cm + algoParam.growParam.maxLineSkipNum = 5; + algoParam.growParam.yDeviation_max = 20.0; + algoParam.growParam.maxSkipDistance = 20.0; + algoParam.growParam.zDeviation_max = algoParam.bagParam.bagH / 2; //袋子高度1/2 + algoParam.growParam.minLTypeTreeLen = 50.0; //mm + algoParam.growParam.minVTypeTreeLen = 50.0; //mm + } + else if ( (grp >= 13) && (grp <= 14)) + { + algoParam.bagParam.bagL = 750; //袋子长65cm + algoParam.bagParam.bagW = 450; //袋子宽40cm + algoParam.bagParam.bagH = 160; //袋子高16cm + algoParam.growParam.maxLineSkipNum = 5; + algoParam.growParam.yDeviation_max = 20.0; + algoParam.growParam.maxSkipDistance = 20.0; + algoParam.growParam.zDeviation_max = algoParam.bagParam.bagH / 2; //袋子高度1/2 + algoParam.growParam.minLTypeTreeLen = 50.0; //mm + algoParam.growParam.minVTypeTreeLen = 50.0; //mm + } + else + { + algoParam.bagParam.bagL = 650; //袋子长65cm + algoParam.bagParam.bagW = 450; //袋子宽40cm + algoParam.bagParam.bagH = 160; //袋子高16cm + algoParam.growParam.maxLineSkipNum = 5; + algoParam.growParam.yDeviation_max = 20.0; + algoParam.growParam.maxSkipDistance = 20.0; + algoParam.growParam.zDeviation_max = algoParam.bagParam.bagH / 2; //袋子高度1/2 + algoParam.growParam.minLTypeTreeLen = 50.0; //mm + algoParam.growParam.minVTypeTreeLen = 50.0; //mm + } +#if BAG_ALGO_USE_CORNER_FEATURE + algoParam.cornerParam.cornerTh = 30; //45度角 + algoParam.cornerParam.scale = algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8; + algoParam.cornerParam.minEndingGap = algoParam.bagParam.bagW / 4; + algoParam.cornerParam.jumpCornerTh_1 = 60; + algoParam.cornerParam.jumpCornerTh_2 = 15; +#else + algoParam.slopeParam.LSlopeZWin = 10.0; + algoParam.slopeParam.validSlopeH = 10.0; + algoParam.slopeParam.minLJumpH = 20.0; + algoParam.slopeParam.minEndingGap = algoParam.bagParam.bagW / 4; ///当没有中间袋子，只有左右袋子时，有两组Ending。此参数确定两个ending最小间距 + algoParam.valleyPara.valleyMinH = 10.0; + algoParam.valleyPara.valleyMaxW = 80.0; //取袋子宽度的1/5 +#endif + if (grp == 7) { char calibFile[250]; sprintf_s(calibFile, "F:\\上古\\编织袋数据\\点云8_广东1213-1215数据\\ground_calib_para.txt"); poseCalibPara = _readCalibPara(calibFile); } + else if (grp == 10) + { + char calibFile[250]; + sprintf_s(calibFile, "F:\\ShangGu\\编织袋数据\\点云11_盐袋\\ground_calib_para.txt"); + poseCalibPara = _readCalibPara(calibFile); + } + else if (grp == 11) + { + char calibFile[250]; + sprintf_s(calibFile, "F:\\ShangGu\\编织袋数据\\点云12_盐袋\\ground_calib_para.txt"); + poseCalibPara = _readCalibPara(calibFile); + } + else if (grp == 12) + { + char calibFile[250]; + sprintf_s(calibFile, "F:\\ShangGu\\编织袋数据\\点云13_现场测试\\ground_calib_para.txt"); + poseCalibPara = _readCalibPara(calibFile); + } + else if ( grp == 13) + { + char calibFile[250]; + sprintf_s(calibFile, "F:\\ShangGu\\编织袋数据\\编织袋RGBD识别方向\\ground_calib_para.txt"); + poseCalibPara = _readCalibPara(calibFile); + } + else if (grp == 14) + { + char calibFile[250]; + sprintf_s(calibFile, "F:\\ShangGu\\编织袋数据\\编织袋RGBD识别方向_2\\ground_calib_para.txt"); + poseCalibPara = _readCalibPara(calibFile); + } for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) { - //fidx = 102; - int lineNum = 0; - float lineV = 0.0f; - int dataCalib = 0; - int maxTimeStamp = 0; - int clockPerSecond = 0; - sprintf_s(_scan_file, "%sLaserLine%d_grid.txt", dataPath[grp], fidx); - SVzNL3DLaserLine* laser3DPoints = vzReadLaserScanPointFromFile_XYZ(_scan_file, &lineNum, &lineV, &dataCalib, &maxTimeStamp, &clockPerSecond); - if (laser3DPoints == NULL) - continue; - - if (grp < 10) //正面抓取 + fidx = 10; + if (grp < TEST_TOP_VIEW_GROUP) //正面抓取 { + int lineNum = 0; + float lineV = 0.0f; + int dataCalib = 0; + int maxTimeStamp = 0; + int clockPerSecond = 0; + sprintf_s(_scan_file, "%sLaserLine%d_grid.txt", dataPath[grp], fidx); + SVzNL3DLaserLine* laser3DPoints = vzReadLaserScanPointFromFile_XYZ(_scan_file, &lineNum, &lineV, &dataCalib, &maxTimeStamp, &clockPerSecond); + if (laser3DPoints == NULL) + continue; + algoParam.filterParam.continuityTh = 20.0; //噪声滤除。当相邻点的z跳变大于此门限时，检查是否为噪声。若长度小于outlierLen， 视为噪声 algoParam.filterParam.outlierTh = 5; @@ -1851,7 +2736,8 @@ int main() sprintf_s(_dbg_file, "%sresult\\LaserLine%d_result_img.png", dataPath[grp], fidx); cv::String imgName(_dbg_file); double rpy[3] = { -30, 15, 0 }; //{ 0,-45, 0 }; // - _genXOYProjectionImage(imgName, laser3DPoints, lineNum, objOps, rpy); + double angleDrawLen = algoParam.bagParam.bagL / 3; + _genXOYProjectionImage(imgName, laser3DPoints, lineNum, objOps, rpy, angleDrawLen); #endif #if 0 //量化成2D图像作为训练样本 @@ -1872,8 +2758,131 @@ int main() #endif printf("%s: %d(ms)!\n", _scan_file, (int)(t2 - t1)); } - else if (grp >= 10) //侧面抓取 + else if (grp < TEST_TOP_ORIEN_GROUP) { + double FBVldPtRatioTh; + + SSG_hsvCmpParam colorCmpParam; //色度和色饱和度比较门限，小于门限为同一颜色 + colorCmpParam.hueTh = 15.0; + colorCmpParam.saturateTh = 120.0; //60 + colorCmpParam.FBVldPtRatioTh = 0.075; //正反两面有效颜色点的比例门限 + colorCmpParam.frontVldPtGreater = true; //true：有效颜色比例高的点的是正面；false：有效颜色比例高的点的是反面 + colorCmpParam.front_upVldPtGreater = false;//true：有效颜色比例高的点的是正面朝上；false：有效颜色比例高的点的是正面朝下 + colorCmpParam.back_upVldPtGreater = true; //true：有效颜色比例高的点的是反面朝上；false：有效颜色比例高的点的是反面朝下 + RGB rgbColorPattern = {36, 165, 208}; //用于区分袋子方向的颜色 + double frontColorTemplate[RGN_HIST_SIZE] = { + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 + }; + + double backColorTemplate[RGN_HIST_SIZE] = { + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 + }; + + int lineNum = 0; + float lineV = 0.0f; + int dataCalib = 0; + int maxTimeStamp = 0; + int clockPerSecond = 0; + sprintf_s(_scan_file, "%s%d-RGB点云.txt", dataPath[grp], fidx); + SVzNLXYZRGBDLaserLine* laser3DPoints = vzReadLaserScanPointFromFile_XYZRGB( + _scan_file, + &lineNum, + &lineV, + &dataCalib, + &maxTimeStamp, + &clockPerSecond); + if (laser3DPoints == NULL) + continue; + + algoParam.filterParam.continuityTh = 20.0; //噪声滤除。当相邻点的z跳变大于此门限时，检查是否为噪声。若长度小于outlierLen， 视为噪声 + algoParam.filterParam.outlierTh = 5; + + long t1 = GetTickCount64(); + + for (int i = 0; i < lineNum; i++) + { + if (i == 14) + int kkk = 1; + //行处理 + //调平，去除地面 + sg_lineDataR_RGBD(&laser3DPoints[i], poseCalibPara.planeCalib, poseCalibPara.planeHeight); + } +#if 0 + //产生一个调平后的2D图像，核对颜色 + char _tst_file[256]; + sprintf_s(_tst_file, "%sresult\\LaserLine%d_2D_img.png", dataPath[grp], fidx); + _RGBDto2DImage(_tst_file, laser3DPoints, lineNum); +#endif + int errCode = 0; + std::vector objOps; +#if OUTPUT_DEBUG + std::vector> debug_bagPositionCloud; +#endif + sg_getBagPositionAndOrientation( + laser3DPoints, + lineNum, + algoParam, + poseCalibPara, + colorCmpParam, + rgbColorPattern, + frontColorTemplate, + backColorTemplate, + objOps, +#if OUTPUT_DEBUG + debug_bagPositionCloud, +#endif + &errCode); + + long t2 = GetTickCount64(); + + char _dbg_file[256]; +#if 1 +#if OUTPUT_DEBUG + sprintf_s(_dbg_file, "%sresult\\LaserLine%d_bagPosition_result.txt", dataPath[grp], fidx); + _outputScanDataFile_obj_vector(_dbg_file, debug_bagPositionCloud, objOps); + +#endif + + sprintf_s(_dbg_file, "%sresult\\LaserLine%d_result.txt", dataPath[grp], fidx); + _outputRGBDScanDataFile_RGBD_obj(_dbg_file, laser3DPoints, lineNum, lineV, maxTimeStamp, clockPerSecond, objOps); + + sprintf_s(_dbg_file, "%sresult\\LaserLine%d_result_img.png", dataPath[grp], fidx); + cv::String imgName(_dbg_file); + double rpy[3] = { -30, 15, 0 }; //{ 0,-45, 0 }; // + double angleDrawLen = algoParam.bagParam.bagL / 3; + _genXOYProjectionImage_RGBD(imgName, laser3DPoints, lineNum, objOps, rpy, angleDrawLen); +#endif +#if 0 + //量化成2D图像作为训练样本 + cv::Mat project2D_img; + double z_scale = 3.0; + double xy_scale = 2.0; + project2DWithInterpolate(project2D_img, laser3DPoints, lineNum, xy_scale, z_scale); + cv::Mat project2D_color; + cv::applyColorMap(project2D_img, project2D_color, cv::COLORMAP_JET); + sprintf_s(_dbg_file, "%simg2D_JET\\LaserLine%d_img2D.png", dataPath[grp], fidx); + cv::String img2DName(_dbg_file); + cv::imwrite(img2DName, project2D_color); + cv::Mat project2D_color_HOT; + cv::applyColorMap(project2D_img, project2D_color_HOT, cv::COLORMAP_HOT); + sprintf_s(_dbg_file, "%simg2D\\LaserLine%d_img2D.png", dataPath[grp], fidx); + cv::String img2DName_HOT(_dbg_file); + cv::imwrite(img2DName_HOT, project2D_color_HOT); +#endif + printf("%s: %d(ms)!\n", _scan_file, (int)(t2 - t1)); + } + else// if (grp >= TEST_TOP_VIEW_GROUP) //侧面抓取 + { + int lineNum = 0; + float lineV = 0.0f; + int dataCalib = 0; + int maxTimeStamp = 0; + int clockPerSecond = 0; + sprintf_s(_scan_file, "%sLaserLine%d_grid.txt", dataPath[grp], fidx); + SVzNL3DLaserLine* laser3DPoints = vzReadLaserScanPointFromFile_XYZ(_scan_file, &lineNum, &lineV, &dataCalib, &maxTimeStamp, &clockPerSecond); + if (laser3DPoints == NULL) + continue; + algoParam.filterParam.continuityTh = 5.0; //噪声滤除。当相邻点的距离大于此门限时，检查是否为噪声。若长度小于outlierLen， 视为噪声 algoParam.filterParam.outlierTh = 5; diff --git a/baseAlgorithm/baseAlgorithm.vcxproj b/baseAlgorithm/baseAlgorithm.vcxproj index 42437db..de7d070 100644 --- a/baseAlgorithm/baseAlgorithm.vcxproj +++ b/baseAlgorithm/baseAlgorithm.vcxproj @@ -166,6 +166,7 @@ + diff --git a/sourceCode/SG_bagPositioning.cpp b/sourceCode/SG_bagPositioning.cpp index fbb8e6f..c52dd4b 100644 --- a/sourceCode/SG_bagPositioning.cpp +++ b/sourceCode/SG_bagPositioning.cpp @@ -12,6 +12,13 @@ void sg_lineDataR(SVzNL3DLaserLine* a_line, lineDataRT( a_line, camPoseR, groundH); } +void sg_lineDataR_RGBD(SVzNLXYZRGBDLaserLine* a_line, + const double* camPoseR, + double groundH) +{ + lineDataRT_RGBD(a_line, camPoseR, groundH); +} + //ɨߴдֱȡ void sg_bagPositioning_lineProc( SVzNL3DLaserLine* a_line, @@ -2289,7 +2296,7 @@ void sg_getBagPosition( cv::imwrite("originMask.png", oriMaskImage); #endif - int vldRgnSize = (int)((algoParam.bagParam.bagL / 2) * (algoParam.bagParam.bagW / 2) / (scale * scale)); + int vldRgnSize = (int)((algoParam.bagParam.bagL * algoParam.bagParam.bagW / 2) / (scale * scale)); for (int rgnid = 0, rgn_max = labelRgns.size(); rgnid < rgn_max; rgnid++) { SSG_Region* a_rgn = &labelRgns[rgnid]; @@ -3807,6 +3814,440 @@ if (i == 2) } } +typedef struct +{ + int vldNum[RGN_HIST_SIZE]; + int totalNum[RGN_HIST_SIZE]; +}_rgnHSVHistInfo; + +HSV RGBtoHSV(int r, int g, int b) { + double r_ = r / 255.0; + double g_ = g / 255.0; + double b_ = b / 255.0; + double max_ = std::max(r_, std::max(g_, b_)); + double min_ = std::min(r_, std::min(g_, b_)); + double delta = max_ - min_; + double h, s, v; + v = max_; + s = (max_ == 0) ? 0 : (delta / max_); + if (delta == 0) //˴޸ġ趨һ + { + h = -1; // δ壬κֵͨΪ0NaN + } + else + { + if (r_ == max_) { + h = (g_ - b_) / delta; + } + else if (g_ == max_) { + h = 2 + (b_ - r_) / delta; + } + else { // b_ == max_ + h = 4 + (r_ - g_) / delta; + } + h *= 60; // hֵ׼[0,360] + if (h < 0) { + h += 360; // ȷh[0,360]Χ + } + } + HSV hsv; + hsv.h = h; + hsv.s = s * 255;// sֵ׼[0,255] + hsv.v = v * 255;// vֵ׼[0,255] + return hsv; +} + +bool isSameColor(HSV hsvPattern, HSV hsvPt, const SSG_hsvCmpParam colorCmpParam) +{ + if (((hsvPattern.h < 0) && (hsvPt.h >= 0)) || ((hsvPattern.h >= 0) && (hsvPt.h < 0))) + return false; + + if ((hsvPattern.h < 0) && (hsvPt.h < 0)) + return true; + + double h_diff = hsvPt.h - hsvPt.h; + if (h_diff < -180) + h_diff = h_diff + 360; + else if (h_diff > 180) + h_diff = h_diff - 360; + h_diff = abs(h_diff); + double s_diff = abs(hsvPattern.s - hsvPt.s); + if ( (h_diff < colorCmpParam.hueTh) && (s_diff < colorCmpParam.saturateTh)) + return true; + else + return false; +} + +double cosSimilarity(const double* colorTemplate, double* sampleData) +{ + double sumA = 0, sumB = 0, sumAB = 0; + for (int i = 0; i < RGN_HIST_SIZE; i++) + { + sumA += colorTemplate[i] * colorTemplate[i]; + sumAB += colorTemplate[i] * sampleData[i]; + sumB = sampleData[i] * sampleData[i]; + } + sumA = sqrt(sumA); + sumB = sqrt(sumB); + sumA = sumA * sumB; + if (sumA <= 1e-4) + return 0; + return (sumAB / sumA); +} + +double cosSimilarityInv(const double* colorTemplate, double* sampleData) +{ + double sumA = 0, sumB = 0, sumAB = 0; + for (int i = 0; i < RGN_HIST_SIZE; i++) + { + sumA += colorTemplate[i] * colorTemplate[i]; + sumAB += colorTemplate[i] * sampleData[RGN_HIST_SIZE - 1 - i]; + sumB = sampleData[RGN_HIST_SIZE - 1 - i] * sampleData[RGN_HIST_SIZE -1 - i]; + } + sumA = sqrt(sumA); + sumB = sqrt(sumB); + sumA = sumA * sumB; + if (sumA <= 1e-4) + return 0; + return (sumAB / sumA); +} + +///RGBD gridʽԽˮƽʹֱĴ +///1Ѱұ߽ +///2ߵ㿪ʼ +/// ֯³ +void sg_getBagPositionAndOrientation( + SVzNLXYZRGBDLaserLine* laser3DPoints, + int lineNum, + //std::vector& all_vLineFeatures, + //std::vector>& noisePts, + const SG_bagPositionParam algoParam, + const SSG_planeCalibPara poseCalibPara, + const SSG_hsvCmpParam colorCmpParam, + const RGB rgbColorPattern, //ִӷɫ + const double frontColorTemplate[RGN_HIST_SIZE], + const double backColorTemplate[RGN_HIST_SIZE], + std::vector& objOps, +#if OUTPUT_DEBUG + std::vector>& bagPositionCloudPts, +#endif + int* errCode) +{ + *errCode = 0; + if (lineNum == 0) + { + *errCode = SG_ERR_3D_DATA_INVLD; + return; + } + //ǷΪդʽ + //RGBDƷӦRGB͵ƣRGBתHSV + std::vector cloudPts; + std::vector> hsvPts; + bool dataValid = true; + int linePtNum = laser3DPoints[0].nPointCnt; + for (int line = 0; line < lineNum; line++) + { + if (laser3DPoints[line].nPointCnt > 0) + { + std::vector line_hsv; + line_hsv.resize(laser3DPoints[line].nPointCnt); + SVzNL3DPosition* p3DPoint = (SVzNL3DPosition*)malloc(sizeof(SVzNL3DPosition) * laser3DPoints[line].nPointCnt); + memset(p3DPoint, 0, sizeof(SVzNL3DPosition) * laser3DPoints[line].nPointCnt); + for (int i = 0; i < laser3DPoints[line].nPointCnt; i++) + { + if ((line == 497) && (i >= 902)) + int kkk = 1; + p3DPoint[i].pt3D.x = laser3DPoints[line].p3DPoint[i].x; + p3DPoint[i].pt3D.y = laser3DPoints[line].p3DPoint[i].y; + p3DPoint[i].pt3D.z = laser3DPoints[line].p3DPoint[i].z; + p3DPoint[i].nPointIdx = i; + + unsigned int nRGB = laser3DPoints[line].p3DPoint[i].nRGB; + + int r = nRGB & 0xff; + nRGB >>= 8; + int g = nRGB & 0xff; + nRGB >>= 8; + int b = nRGB & 0xff; + line_hsv[i] = RGBtoHSV(r,g,b); + } + + SVzNL3DLaserLine a_line; + a_line.nPositionCnt = laser3DPoints[line].nPointCnt; + a_line.nTimeStamp = 0; + a_line.p3DPosition = p3DPoint; + cloudPts.push_back(a_line); + hsvPts.push_back(line_hsv); + } + if (linePtNum != laser3DPoints[line].nPointCnt) + dataValid = false; + } + + if (false == dataValid) + { + *errCode = SG_ERR_3D_DATA_INVLD; + //ͷڴ + for (int i = 0, i_max = cloudPts.size(); i < i_max; i++) + free(cloudPts[i].p3DPosition); + return; + } + + //Ŀ + std::vector peakRgn; + sg_getBagPosition( + &cloudPts[0], + cloudPts.size(), + algoParam, + poseCalibPara, + peakRgn); + + //ͷڴ + for (int i = 0, i_max = cloudPts.size(); i < i_max; i++) + { +#if OUTPUT_DEBUG + std::vector< SVzNL3DPosition> out_line; + int nPtCounter = cloudPts[i].nPositionCnt; + for (int j = 0; j < nPtCounter; j++) + out_line.push_back(cloudPts[i].p3DPosition[j]); + bagPositionCloudPts.push_back(out_line); +#endif + free(cloudPts[i].p3DPosition); + cloudPts[i].p3DPosition = NULL; + } + + //ͶԭϵԱ۱궨ȷ + for (int i = 0; i < lineNum; i++) + sg_lineDataR_RGBD(&laser3DPoints[i], poseCalibPara.invRMatrix, -1); + + //ɫתHSV + HSV hsvPattern = RGBtoHSV(rgbColorPattern.r, rgbColorPattern.g, rgbColorPattern.b); + //ɫͳƴȡcenterPosΪĵģ0.8*Width, 0.8*HeightķΧڵĵͳ + int objNum = peakRgn.size(); + if (objNum > 0) + { + //ˮƽõľΪ׼ģλãˮƽΪȣֱΪ + std::vector y_th; + y_th.resize(objNum); + std::vector x_th; + x_th.resize(objNum); + std::vector x_slice; + x_slice.resize(objNum); + for (int m = 0; m < objNum; m++) + { + y_th[m] = peakRgn[m].objSize.dHeight * 0.25; + x_th[m] = peakRgn[m].objSize.dWidth * 0.45; + x_slice[m] = peakRgn[m].objSize.dWidth / 16.0; + } + + std::vector rgnROIs; + rgnROIs.resize(objNum); + for (int i = 0; i < objNum; i++) + { + //һROIٴ + //ˮƽõľΪ׼ģλ + SVzNL2DPointD vec[4]; + vec[0].x = -peakRgn[i].objSize.dWidth / 2; + vec[0].y = -peakRgn[i].objSize.dHeight / 2; + vec[1].x = peakRgn[i].objSize.dWidth / 2; + vec[1].y = -peakRgn[i].objSize.dHeight / 2; + vec[2].x = peakRgn[i].objSize.dWidth / 2; + vec[2].y = peakRgn[i].objSize.dHeight / 2; + vec[3].x = -peakRgn[i].objSize.dWidth / 2; + vec[3].y = peakRgn[i].objSize.dHeight / 2; + //ת + const double deg2rad = PI / 180.0; + const double yaw = peakRgn[i].centerPos.z_yaw * deg2rad; + double cy = cos(yaw); double sy = sin(yaw); + for (int m = 0; m < 4; m++) + { + double x = vec[m].x * cy + vec[m].y * sy; //ͼϵYŷϵ෴ + double y = -vec[m].x * sy + vec[m].y * cy; + vec[m].x = x + peakRgn[i].centerPos.x; + vec[m].y = y + peakRgn[i].centerPos.y; + } + //ROI + SSG_ROIRectD a_roi; + a_roi.left = vec[0].x; + a_roi.right = vec[0].x; + a_roi.top = vec[0].y; + a_roi.bottom = vec[0].y; + for (int m = 1; m < 4; m++) + { + a_roi.left = a_roi.left > vec[m].x ? vec[m].x : a_roi.left; + a_roi.right = a_roi.right < vec[m].x ? vec[m].x : a_roi.right; + a_roi.top = a_roi.top > vec[m].y ? vec[m].y : a_roi.top; + a_roi.bottom = a_roi.bottom < vec[m].y ? vec[m].y : a_roi.bottom; + } + rgnROIs[i] = a_roi; + } + + std::vector<_rgnHSVHistInfo> rgnHists; //ͳ + rgnHists.resize(objNum); + //ͳ + for (int m = 0; m < objNum; m++) + { + for (int j = 0; j < 16; j++) + { + rgnHists[m].totalNum[j] = 0; + rgnHists[m].vldNum[j] = 0; + } + } + + //еͶ䣬ע˴ʹԭʼ + for (int line = 0; line < lineNum; line++) + { + for (int j = 0; j < linePtNum; j++) + { + SVzNLPointXYZRGBA* a_pt = &laser3DPoints[line].p3DPoint[j]; + if (a_pt->z < 1e-4) + continue; + if ((line == 497) && (j >= 902)) + { + HSV a_hsv = hsvPts[line][j]; + int kkk = 1; + } + for (int m = 0; m < objNum; m++) + { + //ǷROI + if ((a_pt->x >= rgnROIs[m].left) && (a_pt->x <= rgnROIs[m].right) && + (a_pt->y >= rgnROIs[m].top) && (a_pt->y <= rgnROIs[m].bottom)) + { + //Ͷ + //ת,ʱǶ + const double deg2rad = PI / 180.0; + double rotateAngle = - peakRgn[m].centerPos.z_yaw * deg2rad; + double cy = cos(rotateAngle); double sy = sin(rotateAngle); + double x = a_pt->x - peakRgn[m].centerPos.x; + double y = a_pt->y - peakRgn[m].centerPos.y; + double rx = x * cy + y * sy; + double ry = -x * sy + y * cy; + if ((rx >= -x_th[m]) && (rx <= x_th[m]) && (ry >= -y_th[m]) && (ry <= y_th[m])) + { + if (m == 0) + int kkk = 1; + else if (m == 1) + int kkk = 1; + else if (m == 2) + int kkk = 1; + else if (m == 3) + int kkk = 1; + double rx_abs = abs(rx); + int hist_idx = rx_abs / x_slice[m]; + if (hist_idx >= RGN_HIST_SIZE / 2) + hist_idx = RGN_HIST_SIZE / 2 - 1; + if (rx < 0) + hist_idx = -hist_idx + RGN_HIST_SIZE / 2 - 1; + else + hist_idx = hist_idx + RGN_HIST_SIZE / 2; + //ж + bool isSame = isSameColor(hsvPattern, hsvPts[line][j], colorCmpParam); + if (true == isSame) + rgnHists[m].vldNum[hist_idx] ++; + rgnHists[m].totalNum[hist_idx] ++; + } + } + } + } + } + + + for (int m = 0; m < objNum; m++) + { + double normalizeHist[RGN_HIST_SIZE]; + //ɹһ + int totalNum=0, totalVldNum=0, upVldNum=0, dwnVldNum=0; + for (int j = 0; j < RGN_HIST_SIZE; j++) + { + totalNum += rgnHists[m].totalNum[j]; + totalVldNum += rgnHists[m].vldNum[j]; + + if (j < (RGN_HIST_SIZE / 2-2)) + dwnVldNum += rgnHists[m].vldNum[j]; + else if (j >= (RGN_HIST_SIZE / 2+2)) + upVldNum += rgnHists[m].vldNum[j]; + + if (rgnHists[m].totalNum[j] > 0) + normalizeHist[j] = (double)rgnHists[m].vldNum[j] / (double)rgnHists[m].totalNum[j]; + else + normalizeHist[j] = 0; + } + + SSG_peakOrienRgnInfo a_obj; + a_obj.centerPos = peakRgn[m].centerPos; + a_obj.objSize = peakRgn[m].objSize; + a_obj.pkRgnIdx = peakRgn[m].pkRgnIdx; + a_obj.pos2D = peakRgn[m].pos2D; + a_obj.orienFlag = 0; //δ֪ + //ƶȱȽ +#if 0 + double frontSim = cosSimilarity(frontColorTemplate, normalizeHist); + double frontSim_inv = cosSimilarityInv(frontColorTemplate, normalizeHist); + double backSim = cosSimilarity(backColorTemplate, normalizeHist); + double backSim_inv = cosSimilarityInv(backColorTemplate, normalizeHist); + if ((frontSim > frontSim_inv) && (frontSim > backSim) && (frontSim > backSim_inv) && (frontSim > 0.5)) + { + a_obj.orienFlag = 1; + } + else if ((frontSim_inv > frontSim) && (frontSim_inv > backSim) && (frontSim_inv > backSim_inv) && (frontSim_inv > 0.5)) + { + a_obj.orienFlag = 1; + a_obj.centerPos.z_yaw += 180; + if (a_obj.centerPos.z_yaw >= 360) + a_obj.centerPos.z_yaw = a_obj.centerPos.z_yaw - 360; + } + else if ((backSim > frontSim) && (backSim > frontSim_inv) && (backSim > backSim_inv) && (backSim > 0.5)) + { + a_obj.orienFlag = 2; + } + else if ((backSim_inv > frontSim) && (backSim_inv > frontSim_inv) && (backSim_inv > backSim) && (backSim_inv > 0.5)) + { + a_obj.orienFlag = 2; + a_obj.centerPos.z_yaw += 180; + if (a_obj.centerPos.z_yaw >= 360) + a_obj.centerPos.z_yaw = a_obj.centerPos.z_yaw - 360; + } +#else + double vldPtRatio = (double)totalVldNum / (double)totalNum; + bool isFront; + if (true == colorCmpParam.frontVldPtGreater) + isFront = vldPtRatio > colorCmpParam.FBVldPtRatioTh ? true : false; + else + isFront = vldPtRatio > colorCmpParam.FBVldPtRatioTh ? false : true; + + bool orienIsNormal; + if (true == isFront) + { + if (true == colorCmpParam.front_upVldPtGreater) + orienIsNormal = upVldNum > dwnVldNum ? true : false; + else + orienIsNormal = upVldNum > dwnVldNum ? false : true; + } + else + { + if (true == colorCmpParam.back_upVldPtGreater) + orienIsNormal = upVldNum > dwnVldNum ? true : false; + else + orienIsNormal = upVldNum > dwnVldNum ? false : true; + } + + if (true == isFront) + a_obj.orienFlag = 1; + else + a_obj.orienFlag = 2; + + if (false == orienIsNormal) + { + a_obj.centerPos.z_yaw += 180; + if (a_obj.centerPos.z_yaw >= 360) + a_obj.centerPos.z_yaw = a_obj.centerPos.z_yaw - 360; + } +#endif + objOps.push_back(a_obj); + } + } + +} + bool compareByXPos(const SSG_featureTree& a, const SSG_featureTree& b) { return (a.roi.left + a.roi.right) < (b.roi.left + b.roi.right); } diff --git a/sourceCode/SG_bagPositioning_Export.h b/sourceCode/SG_bagPositioning_Export.h index 07891c2..867a6e2 100644 --- a/sourceCode/SG_bagPositioning_Export.h +++ b/sourceCode/SG_bagPositioning_Export.h @@ -6,8 +6,9 @@ # define SG_APISHARED_EXPORT __declspec(dllimport) #endif -#define OUTPUT_DEBUG 0 +#define OUTPUT_DEBUG 1 #define BAG_ALGO_USE_CORNER_FEATURE 1 +#define RGN_HIST_SIZE 16 //ĿɫͳƵĿ #include "SG_baseDataType.h" #include @@ -27,6 +28,16 @@ typedef struct //SSG_polarScanParam polarScanParam; }SG_bagPositionParam; +typedef struct +{ + double hueTh; + double saturateTh; + double FBVldPtRatioTh; //Чɫı + bool frontVldPtGreater; //trueЧɫߵĵ棻falseЧɫߵĵǷ + bool front_upVldPtGreater;//trueЧɫߵĵ泯ϣfalseЧɫߵĵ泯 + bool back_upVldPtGreater; //trueЧɫߵĵǷ泯ϣfalseЧɫߵĵǷ泯 +}SSG_hsvCmpParam; + typedef struct { double jumpTh; //ֳйأΪ̵Ŀ׶Z @@ -34,7 +45,13 @@ typedef struct double baseHoleDist; //׶ľ }SSG_stackBaseParam; -SG_APISHARED_EXPORT void sg_lineDataR(SVzNL3DLaserLine* a_line, +//ݵƽ +SG_APISHARED_EXPORT void sg_lineDataR( + SVzNL3DLaserLine* a_line, + const double* camPoseR, + double groundH); +SG_APISHARED_EXPORT void sg_lineDataR_RGBD( + SVzNLXYZRGBDLaserLine* a_line, const double* camPoseR, double groundH); @@ -56,6 +73,23 @@ SG_APISHARED_EXPORT void sg_getBagPosition( const SSG_planeCalibPara poseCalibPara, std::vector& objOps); +SG_APISHARED_EXPORT void sg_getBagPositionAndOrientation( + SVzNLXYZRGBDLaserLine* laser3DPoints, + int lineNum, + //std::vector& all_vLineFeatures, + //std::vector>& noisePts, + const SG_bagPositionParam algoParam, + const SSG_planeCalibPara poseCalibPara, + const SSG_hsvCmpParam colorCmpParam, + const RGB rgbColorPattern, //ִӷɫ + const double frontColorTemplate[RGN_HIST_SIZE], + const double backColorTemplate[RGN_HIST_SIZE], + std::vector& objOps, +#if OUTPUT_DEBUG + std::vector>& bagPositionCloudPts, +#endif + int* errCode); + SG_APISHARED_EXPORT void sg_sideBagPosition( SVzNL3DLaserLine* laser3DPoints, int lineNum, diff --git a/sourceCode/SG_baseAlgo_Export.h b/sourceCode/SG_baseAlgo_Export.h index 552b2ec..32c2087 100644 --- a/sourceCode/SG_baseAlgo_Export.h +++ b/sourceCode/SG_baseAlgo_Export.h @@ -75,6 +75,14 @@ SG_APISHARED_EXPORT void sg_getLineLocalPeaks( std::vector< SSG_basicFeature1D>& localMax, std::vector< SSG_basicFeature1D>& localMin); +SG_APISHARED_EXPORT void sg_getFlatLineLocalPeaks_vector( + std::vector& lineData, + int lineIdx, + const double scaleWin, + const double minPkHeighth, + const double holeR, + std::vector< SSG_basicFeature1D>& localMax); + SG_APISHARED_EXPORT void sg_getLineDownJumps( SVzNL3DPosition* lineData, int dataSize, @@ -88,6 +96,13 @@ SG_APISHARED_EXPORT void sg_getFeatureGrowingTrees( std::vector& trees, SSG_treeGrowParam growParam); +SG_APISHARED_EXPORT void sg_lineFeaturesGrowing( + int lineIdx, + bool isLastLine, + std::vector& features, + std::vector& trees, + SSG_treeGrowParam growParam); + //ending SG_APISHARED_EXPORT void sg_getEndingGrowingTrees( std::vector& lineEndings, @@ -212,6 +227,10 @@ SG_APISHARED_EXPORT void sg_getLineRigthAngleFeature( SG_APISHARED_EXPORT SVzNL3DRangeD sg_getScanDataROI( SVzNL3DLaserLine* laser3DPoints, int lineNum); +//ɨROI: vecotrʽ +SG_APISHARED_EXPORT SVzNL3DRangeD sg_getScanDataROI_vector( + std::vector< std::vector>& scanLines +); //XYƽֱ SG_APISHARED_EXPORT void lineFitting( @@ -264,4 +283,18 @@ SG_APISHARED_EXPORT void eulerToRotationMatrixZYX(const SSG_EulerAngles& angles, SG_APISHARED_EXPORT void lineDataRT( SVzNL3DLaserLine* a_line, const double* camPoseR, - double groundH); \ No newline at end of file + double groundH); +SG_APISHARED_EXPORT void lineDataRT_vector( + std::vector< SVzNL3DPosition>& a_line, + const double* camPoseR, + double groundH); +SG_APISHARED_EXPORT void lineDataRT_RGBD( + SVzNLXYZRGBDLaserLine* a_line, + const double* camPoseR, + double groundH); + +SG_APISHARED_EXPORT void sg_pointClustering( + std::vector< SVzNL3DPosition>& pts, + double clusterDist, + std::vector>& objClusters //result +); \ No newline at end of file diff --git a/sourceCode/SG_baseDataType.h b/sourceCode/SG_baseDataType.h index c2f44be..5cc63de 100644 --- a/sourceCode/SG_baseDataType.h +++ b/sourceCode/SG_baseDataType.h @@ -2,6 +2,7 @@ #include #include +#include #define PI 3.141592654 @@ -55,6 +56,18 @@ typedef struct double bottom; }SSG_ROIRectD; +struct HSV { + double h; // ɫ (0-360) + double s; // Ͷ (0-1) + double v; // (0-1) +}; + +struct RGB { + int r; + int g; + int b; +}; + #define LINE_FEATURE_NUM 16 #define LINE_FEATURE_UNDEF 0 #define LINE_FEATURE_L_JUMP_H2L 1 @@ -311,6 +324,15 @@ typedef struct SSG_6DOF centerPos; }SSG_peakRgnInfo; +typedef struct +{ + int pkRgnIdx; + SVzNLSizeD objSize; + SVzNL2DPoint pos2D; + SSG_6DOF centerPos; + int orienFlag; //0-δ֪1 - 棬 2- +}SSG_peakOrienRgnInfo; + typedef struct { SVzNL3DPoint sPt; diff --git a/sourceCode/SG_baseFunc.cpp b/sourceCode/SG_baseFunc.cpp index 103bf31..083f0ba 100644 --- a/sourceCode/SG_baseFunc.cpp +++ b/sourceCode/SG_baseFunc.cpp @@ -64,6 +64,66 @@ SVzNL3DRangeD sg_getScanDataROI( return roi; } +SVzNL3DRangeD sg_getScanDataROI_vector(std::vector< std::vector>& scanLines) +{ + SVzNL3DRangeD roi; + roi.xRange = { 0, -1 }; + roi.yRange = { 0, -1 }; + roi.zRange = { 0, -1 }; + + int lineNum = scanLines.size(); + for (int line = 0; line < lineNum; line++) + { + int nPositionCnt = scanLines[line].size(); + for (int i = 0; i < nPositionCnt; i++) + { + SVzNL3DPosition* pt3D = &scanLines[line][i]; + if (pt3D->pt3D.z < 1e-4) + continue; + + if (roi.xRange.max < roi.xRange.min) + { + roi.xRange.min = pt3D->pt3D.x; + roi.xRange.max = pt3D->pt3D.x; + } + else + { + if (roi.xRange.min > pt3D->pt3D.x) + roi.xRange.min = pt3D->pt3D.x; + if (roi.xRange.max < pt3D->pt3D.x) + roi.xRange.max = pt3D->pt3D.x; + } + //y + if (roi.yRange.max < roi.yRange.min) + { + roi.yRange.min = pt3D->pt3D.y; + roi.yRange.max = pt3D->pt3D.y; + } + else + { + if (roi.yRange.min > pt3D->pt3D.y) + roi.yRange.min = pt3D->pt3D.y; + if (roi.yRange.max < pt3D->pt3D.y) + roi.yRange.max = pt3D->pt3D.y; + } + //z + if (roi.zRange.max < roi.zRange.min) + { + roi.zRange.min = pt3D->pt3D.z; + roi.zRange.max = pt3D->pt3D.z; + } + else + { + if (roi.zRange.min > pt3D->pt3D.z) + roi.zRange.min = pt3D->pt3D.z; + if (roi.zRange.max < pt3D->pt3D.z) + roi.zRange.max = pt3D->pt3D.z; + } + } + } + return roi; +} + void lineFitting(std::vector< SVzNL3DPoint>& inliers, double* _k, double* _b) { //Сֱ߲ @@ -876,9 +936,10 @@ SSG_planeCalibPara sg_getPlaneCalibPara( zHist[histPos] ++; } } - //Ϊλۼ std::vector zSumHist; zSumHist.resize(zHistSize); + bool isSame = true; + //Ϊλۼ for (int i = 0; i < zHistSize; i++) { int sumValue = 0; @@ -888,6 +949,17 @@ SSG_planeCalibPara sg_getPlaneCalibPara( sumValue += zHist[j]; } zSumHist[i] = sumValue; + if (i > 0) + { + if (sumValue != zSumHist[i - 1]) + isSame = false; + } + } + if(true == isSame) + { + //ۼӣۼӣۼֵȣ + for (int i = 0; i < zHistSize; i++) + zSumHist[i] = zHist[i]; } //ѰҼֵ @@ -936,6 +1008,8 @@ SSG_planeCalibPara sg_getPlaneCalibPara( pkTop.push_back({pre_i, pre_data}); _state = 2; } + else if(i == (zHistSize-1)) + pkTop.push_back({ i, curr_data }); break; case 2: //½ if (z_diff > 0) // @@ -945,6 +1019,12 @@ SSG_planeCalibPara sg_getPlaneCalibPara( pkBtm.push_back({ pre_i, pre_data }); _state = 1; } + else if (i == (zHistSize - 1)) + { + int pkBtmIdx = pkBtm.size(); + pkBtmBackIndexing[i] = pkBtmIdx; + pkBtm.push_back({ i, curr_data }); + } break; default: _state = 0; @@ -1377,4 +1457,42 @@ void lineDataRT(SVzNL3DLaserLine* a_line, const double* camPoseR, double groundH a_line->p3DPosition[i].pt3D = a_pt; } return; +} +void lineDataRT_vector(std::vector< SVzNL3DPosition>& a_line, const double* camPoseR, double groundH) +{ + for (int i = 0; i < a_line.size(); i++) + { + SVzNL3DPoint a_pt = a_line[i].pt3D; + if (a_pt.z < 1e-4) + continue; + double x = a_pt.x * camPoseR[0] + a_pt.y * camPoseR[1] + a_pt.z * camPoseR[2]; + double y = a_pt.x * camPoseR[3] + a_pt.y * camPoseR[4] + a_pt.z * camPoseR[5]; + double z = a_pt.x * camPoseR[6] + a_pt.y * camPoseR[7] + a_pt.z * camPoseR[8]; + if ((groundH > 0) && (z > groundH)) //ȥ + z = 0; + a_pt.x = x; + a_pt.y = y; + a_pt.z = z; + a_line[i].pt3D = a_pt; + } + return; +} +void lineDataRT_RGBD(SVzNLXYZRGBDLaserLine* a_line, const double* camPoseR, double groundH) +{ + for (int i = 0; i < a_line->nPointCnt; i++) + { + SVzNLPointXYZRGBA a_pt = a_line->p3DPoint[i]; + if (a_pt.z < 1e-4) + continue; + double x = a_pt.x * camPoseR[0] + a_pt.y * camPoseR[1] + a_pt.z * camPoseR[2]; + double y = a_pt.x * camPoseR[3] + a_pt.y * camPoseR[4] + a_pt.z * camPoseR[5]; + double z = a_pt.x * camPoseR[6] + a_pt.y * camPoseR[7] + a_pt.z * camPoseR[8]; + if ((groundH > 0) && (z > groundH)) //ȥ + z = 0; + a_pt.x = x; + a_pt.y = y; + a_pt.z = z; + a_line->p3DPoint[i] = a_pt; + } + return; } \ No newline at end of file diff --git a/sourceCode/SG_featureGrow.cpp b/sourceCode/SG_featureGrow.cpp index 91730fe..a9724b4 100644 --- a/sourceCode/SG_featureGrow.cpp +++ b/sourceCode/SG_featureGrow.cpp @@ -407,6 +407,54 @@ void sg_getFeatureGrowingTrees( } } +void sg_lineFeaturesGrowing( + int lineIdx, + bool isLastLine, + std::vector& features, + std::vector& trees, + SSG_treeGrowParam growParam) +{ + if (features.size() > 0) + { + for (int j = 0, j_max = features.size(); j < j_max; j++) + { + SSG_basicFeature1D& a_feature = features[j]; + if (a_feature.jumpPos2D.x== 207) + int kkk = 1; + bool isMatched = _featureGrowing(a_feature, lineIdx, trees, growParam); + if (false == isMatched) + { + //µ + SSG_featureTree a_newTree; + a_newTree.treeNodes.push_back(a_feature); + a_newTree.treeState = TREE_STATE_ALIVE; + a_newTree.treeType = a_feature.featureType; + a_newTree.sLineIdx = lineIdx; + a_newTree.eLineIdx = lineIdx; + trees.push_back(a_newTree); + } + } + } + //ֹͣ,١ + //ڵΪ1Ƴ + int m_max = trees.size(); + for (int m = m_max - 1; m >= 0; m--) //ӺǰɾӰѭ + { + if (TREE_STATE_ALIVE == trees[m].treeState) + { + int line_diff = abs(lineIdx - trees[m].treeNodes.back().jumpPos2D.x); + if (((growParam.maxLineSkipNum > 0) && (line_diff > growParam.maxLineSkipNum)) || + (true == isLastLine)) + { + trees[m].treeState = TREE_STATE_DEAD; + bool isValid = _invalidateVSlopeTrees(trees[m], growParam.minLTypeTreeLen, growParam.minVTypeTreeLen); + if (false == isValid) + trees.erase(trees.begin() + m); + } + } + } +} + void sg_getEndingGrowingTrees( std::vector& lineEndings, SVzNL3DLaserLine* laser3DPoints, diff --git a/sourceCode/SG_lineFeature.cpp b/sourceCode/SG_lineFeature.cpp index 52d2ab4..72933bd 100644 --- a/sourceCode/SG_lineFeature.cpp +++ b/sourceCode/SG_lineFeature.cpp @@ -149,7 +149,10 @@ if (i == 370) } //˳Ⱥ㣺z޷ -void sg_lineDataRemoveOutlier_changeOriginData(SVzNL3DPosition* lineData, int dataSize, SSG_outlierFilterParam filterParam) +void sg_lineDataRemoveOutlier_changeOriginData( + SVzNL3DPosition* lineData, + int dataSize, + SSG_outlierFilterParam filterParam) { std::vector< SSG_RUN> continueRuns; SSG_RUN a_run = { 0, -1, 0 }; //startIdx, len, lastIdx @@ -1539,6 +1542,804 @@ void sg_getLineLocalPeaks( return; } +#if 0 +typedef struct +{ + double holeLen; + double holeCenterY; + double holeCenterX; + int holeSIdx; + int holeEIdx; +}_HoleInfo; + +typedef struct +{ + double maxZ; + double minZ; + double pk_y; + double pk_x; + int pkIdx; +}_Feature; +//ԵƽֱѰͻ𡣲÷ֶηѰҡ +void sg_getFlatLineLocalPeaks_vector( + std::vector& lineData, + int lineIdx, + const double scaleWin, + const double minPkHeighth, + const double holeR, + std::vector< SSG_basicFeature1D>& localMax) +{ + int dataSize = lineData.size(); + if (dataSize < 2) + return; + + double maxZ = 0; + double subScale = scaleWin / 3.0; + std::vector<_Feature> subFeatures; + int maxIdx = -1; + int minIdx = -1; + bool startVld = false; + double startY = 0; + int preVldPt = 0; + int holePtNum = 0; + _HoleInfo a_hole = { 0.0, 0.0, 0.0, -1, -1 }; + for (int i = 0; i < dataSize; i++) + { + lineData[i].nPointIdx = 0; + if ((lineIdx == 1219) && (i == 2722)) + int kkk = 1; + + if (lineData[i].pt3D.z < 1e-4) + { + if (true == startVld) + holePtNum++; + } + else + { + if (maxZ < lineData[i].pt3D.z) + maxZ = lineData[i].pt3D.z; + + if (false == startVld) + { + startVld = true; + startY = lineData[i].pt3D.y; + } + //׶ + if ((holePtNum > 0) && (preVldPt >= 0)) + { + double holeLen = lineData[i].pt3D.y - lineData[preVldPt].pt3D.y; + if (holeLen > holeR / 3) + { + _HoleInfo currHole; + currHole.holeLen = holeLen; + currHole.holeCenterY = (lineData[i].pt3D.y + lineData[preVldPt].pt3D.y) / 2; + currHole.holeCenterX = (lineData[i].pt3D.x + lineData[preVldPt].pt3D.x) / 2; + currHole.holeSIdx = preVldPt; + currHole.holeEIdx = i; + + if (a_hole.holeSIdx >= 0) + { + //׶ǷԺϲܺϲȡĿ׶ + double hole_dist = currHole.holeCenterY - a_hole.holeCenterY; + if (hole_dist < holeR * 2) + { + a_hole.holeEIdx = currHole.holeEIdx; + a_hole.holeLen = lineData[a_hole.holeEIdx].pt3D.y - lineData[a_hole.holeSIdx].pt3D.y; + a_hole.holeCenterY = (lineData[a_hole.holeEIdx].pt3D.y + lineData[a_hole.holeSIdx].pt3D.y) / 2; + } + else if (a_hole.holeLen < currHole.holeLen) + a_hole = currHole; + + } + else + a_hole = currHole; + } + //¼׶ + holePtNum = 0; + } + + double diffY = lineData[i].pt3D.y - startY; + if (diffY >= subScale) //ֶμ + { + if (a_hole.holeSIdx >= 0) + { + _Feature a_subFeature; + a_subFeature.minZ = -1; + a_subFeature.maxZ = -1; + a_subFeature.pk_y = a_hole.holeCenterY; + a_subFeature.pk_x = a_hole.holeCenterX; + a_subFeature.pkIdx = (a_hole.holeSIdx + a_hole.holeEIdx) / 2; + subFeatures.push_back(a_subFeature); + } + else if (maxIdx >= 0) + { + double depth = lineData[maxIdx].pt3D.z - lineData[minIdx].pt3D.z; + if (depth > minPkHeighth / 2) + { + _Feature a_subFeature; + a_subFeature.minZ = lineData[minIdx].pt3D.z; + a_subFeature.maxZ = lineData[maxIdx].pt3D.z; + a_subFeature.pk_x = lineData[maxIdx].pt3D.x; + a_subFeature.pk_y = lineData[maxIdx].pt3D.y; + a_subFeature.pkIdx = maxIdx; + subFeatures.push_back(a_subFeature); + } + } + //ֶ¿ʼ + maxIdx = -1; + minIdx = -1; + startY = lineData[i].pt3D.y; + a_hole = { 0.0, 0.0, 0.0, -1, -1 }; + } + + if (maxIdx < 0) + maxIdx = i; + else + { + if (lineData[maxIdx].pt3D.z < lineData[i].pt3D.z) + maxIdx = i; + } + if (minIdx < 0) + minIdx = i; + else + { + if (lineData[minIdx].pt3D.z > lineData[i].pt3D.z) + minIdx = i; + } + + //һ + if (i == dataSize - 1) + { + if (a_hole.holeSIdx >= 0) + { + _Feature a_subFeature; + a_subFeature.minZ = -1; + a_subFeature.maxZ = -1; + a_subFeature.pk_x = a_hole.holeCenterX; + a_subFeature.pk_y = a_hole.holeCenterY; + a_subFeature.pkIdx = (a_hole.holeSIdx + a_hole.holeEIdx) / 2; + subFeatures.push_back(a_subFeature); + } + else if (maxIdx >= 0) + { + _Feature a_subFeature; + a_subFeature.minZ = lineData[minIdx].pt3D.z; + a_subFeature.maxZ = lineData[maxIdx].pt3D.z; + a_subFeature.pk_x = lineData[maxIdx].pt3D.x; + a_subFeature.pk_y = lineData[maxIdx].pt3D.y; + a_subFeature.pkIdx = maxIdx; + subFeatures.push_back(a_subFeature); + } + } + preVldPt = i; + } + } + + for (int i = 0, i_max = subFeatures.size(); i < i_max; i++) + { + _Feature* currFeature = &subFeatures[i]; + if (currFeature->pkIdx < 0) + continue; + if (currFeature->maxZ < 0) // ׶ + { + bool isCombined = false; + if (i < i_max - 1) //ϲ + { + _Feature* nxtFeature = &subFeatures[i + 1]; + //׶ǷԺϲܺϲȡĿ׶ + double hole_dist = nxtFeature->pk_y - currFeature->pk_y; + if (hole_dist < holeR*2) //ϲ + { + if (nxtFeature->maxZ < 0) + { + nxtFeature->pkIdx = (currFeature->pkIdx + nxtFeature->pkIdx) / 2; + nxtFeature->pk_x = (currFeature->pk_x + nxtFeature->pk_x) / 2; + nxtFeature->pk_y = (currFeature->pk_y + nxtFeature->pk_y) / 2; + } + currFeature->pkIdx = -1; //ϲ + isCombined = true; + } + } + if (false == isCombined) + { + // + SSG_basicFeature1D a_feature; + a_feature.featureType = LINE_FEATURE_PEAK_TOP; + a_feature.jumpPos2D = { lineIdx, currFeature->pkIdx }; + a_feature.jumpPos.x = currFeature->pk_x; + a_feature.jumpPos.y = currFeature->pk_y; + a_feature.jumpPos.z = maxZ; + localMax.push_back(a_feature); + lineData[currFeature->pkIdx].nPointIdx = LINE_FEATURE_PEAK_TOP; //nPointIdxתʹ + lineData[currFeature->pkIdx].pt3D = a_feature.jumpPos; + } + } + else + { + bool isBigger_1 = true; //ǰһ + bool isBigger_2 = true; //Ⱥһ + double minZ = currFeature->minZ; + + if (i > 0) + { + _Feature* preFeature = &subFeatures[i - 1]; + if (preFeature->maxZ < 0) + { + if(preFeature->pkIdx >= 0) + isBigger_1 = false; + } + else + { + if (currFeature->minZ > preFeature->minZ) + minZ = preFeature->minZ; + if (currFeature->maxZ < preFeature->maxZ) + isBigger_1 = false; + } + } + if (i < i_max - 1) + { + _Feature* nxtFeature = &subFeatures[i + 1]; + if (nxtFeature->maxZ < 0) + { + double hole_dist = nxtFeature->pk_y - currFeature->pk_y; + if (hole_dist >= holeR * 2) + isBigger_2 = false; + else + nxtFeature->pkIdx = -1; //Ŀױϲ + } + else + { + if (currFeature->minZ > nxtFeature->minZ) + minZ = nxtFeature->minZ; + if (currFeature->maxZ < nxtFeature->maxZ) + isBigger_2 = false; + } + } + if ((true == isBigger_1) && (true == isBigger_2)) + { + double z_diff = currFeature->maxZ - minZ; + if (z_diff > minPkHeighth) //ϸͻ + { + SSG_basicFeature1D a_feature; + a_feature.featureType = LINE_FEATURE_PEAK_TOP; + a_feature.jumpPos2D = { lineIdx, currFeature->pkIdx }; + a_feature.jumpPos = lineData[currFeature->pkIdx].pt3D; + localMax.push_back(a_feature); + lineData[currFeature->pkIdx].nPointIdx = LINE_FEATURE_PEAK_TOP; //nPointIdxתʹ + } + } + } + } + return; +} +#else +//ԵƽֱѰͻ +//бѰңѰ䡢б¡Gap +typedef struct +{ + int type; + int ptIdxS; + int ptIdxE; + int sIdx; + int eIdx; + SVzNL3DPoint start; + SVzNL3DPoint end; + double delta_y; + double delta_z; + double k; +}_FeatureInfo; + +int _findSegMaxZPt( + std::vector& lineData, + int searchStart, + bool searchToHead, + double searchWin, + double* meanZ, + bool* foundJump) +{ + int dataSize = lineData.size(); + double start_y = lineData[searchStart].pt3D.y; + double start_z = lineData[searchStart].pt3D.z; + if (start_z < 1e-4) + return -1; + + double max_z = start_z; + int max_idx = searchStart; + double sum_z = max_z; + int sum_num = 1; + int step = searchToHead == true ? -1 : 1; + int idx = searchStart; + while (1) + { + idx += step; + if ((idx <= 0) || (idx >= dataSize)) + break; + + if (lineData[idx].nPointIdx == LINE_FEATURE_NUM) + { + *foundJump = true; + break; + } + double pt_y = lineData[idx].pt3D.y; + double pt_z = lineData[idx].pt3D.z; + if (pt_z > 1e-4) + { + double dist = abs(pt_y - start_y); + if (dist > searchWin) + { + *meanZ = sum_z / sum_num; + return max_idx; + } + else + { + sum_z += pt_z; + sum_num++; + if (max_z < pt_z) + { + max_z = pt_z; + max_idx = idx; + } + } + + } + } + return -1; +} + +void sg_getFlatLineLocalPeaks_vector( + std::vector& lineData, + int lineIdx, + const double scaleWin, + const double minPkHeighth, + const double holeR, + std::vector< SSG_basicFeature1D>& localMax) +{ + int dataSize = lineData.size(); + if (dataSize < 2) + return; + + //٣г + //ĵȥյ + double slopeZDeltaTH = 0.2; //slopez仯ޣڴΪslope + int dwnSample = 1; + int sampleNum = dataSize / dwnSample; + std::vector sampleData; + for (int i = 0; i < sampleNum; i++) + { + SVzNL3DPosition a_sample = lineData[i * dwnSample]; + a_sample.nPointIdx = i * dwnSample; + if(a_sample.pt3D.z > 1e-4) + sampleData.push_back(a_sample); + } + + sampleNum = sampleData.size(); + std::vector< _FeatureInfo> allSubFeatures; + allSubFeatures.resize(sampleNum); + for (int i = 0; i < sampleNum; i++) //ʼ + allSubFeatures[i].type = LINE_FEATURE_UNDEF; + + int segEnd = 0; + for (int i = 0; i < sampleNum-1; i++) + { + SVzNL3DPosition* samplePtr = &sampleData[i]; + if ((lineIdx == 0) && (i == 2290)) + int kkk = 1; + //ǷΪgap + SVzNL3DPosition* nxtSamplePtr = &sampleData[i+1]; + //double d_delta = sqrt(pow(nxtSamplePtr->pt3D.y - samplePtr->pt3D.y, 2) + pow(nxtSamplePtr->pt3D.z - samplePtr->pt3D.z, 2)); + double y_delta = abs(nxtSamplePtr->pt3D.y - samplePtr->pt3D.y); + double z_delta = nxtSamplePtr->pt3D.z - samplePtr->pt3D.z; + if ((y_delta > holeR / 2) || (abs(z_delta) > minPkHeighth)) + { + _FeatureInfo a_subFeature; + a_subFeature.delta_y = y_delta; + a_subFeature.delta_z = z_delta; + a_subFeature.k = z_delta / y_delta; + a_subFeature.ptIdxS = i; + a_subFeature.ptIdxE = i+1; + a_subFeature.sIdx = sampleData[i].nPointIdx; + a_subFeature.eIdx = sampleData[i + 1].nPointIdx; + a_subFeature.start = samplePtr->pt3D; + a_subFeature.end = nxtSamplePtr->pt3D; + if(z_delta > 0) + a_subFeature.type = LINE_FEATURE_L_JUMP_L2H; + else + a_subFeature.type = LINE_FEATURE_L_JUMP_H2L; + allSubFeatures[i] = a_subFeature; + segEnd = i + 1; + } + else + { + //Ѱҳ߶ȶ˵ + for (int j = segEnd; j < sampleNum; j++) + { + SVzNL3DPosition* postSamplePtr = &sampleData[j]; + double deltaY = abs(postSamplePtr->pt3D.y - samplePtr->pt3D.y); + if (deltaY >= scaleWin) + { + double deltaZ = postSamplePtr->pt3D.z - samplePtr->pt3D.z; + if (abs(deltaZ) > slopeZDeltaTH) + int kkk = 1; + _FeatureInfo a_subFeature; + a_subFeature.delta_y = deltaY; + a_subFeature.delta_z = deltaZ; + a_subFeature.k = deltaZ / deltaY; + a_subFeature.ptIdxS = i; + a_subFeature.ptIdxE = j; + a_subFeature.sIdx = sampleData[i].nPointIdx; + a_subFeature.eIdx = sampleData[j].nPointIdx; + a_subFeature.start = samplePtr->pt3D; + a_subFeature.end = postSamplePtr->pt3D; + if ( (abs(a_subFeature.k) > 0.5) && (abs(deltaZ) > slopeZDeltaTH)) + { + if (deltaZ > 0) + a_subFeature.type = LINE_FEATURE_L_SLOPE_L2H; + else + a_subFeature.type = LINE_FEATURE_L_SLOPE_H2L; + } + else + a_subFeature.type = LINE_FEATURE_UNDEF; + allSubFeatures[i] = a_subFeature; + segEnd = j; + break; + } + } + } + } + + + //JUMP, ڵLINE_FEATURE_L_SLOPE_L2HЧ + for (int i = 0; i < sampleNum; i++) + { + if (allSubFeatures[i].type == LINE_FEATURE_L_JUMP_L2H) + { + //ڵLINE_FEATURE_L_SLOPE_L2HЧ + for (int j = i + 1; j < sampleNum; j++) + { + if (allSubFeatures[j].type == LINE_FEATURE_L_SLOPE_L2H) + allSubFeatures[j].type = LINE_FEATURE_UNDEF; + else + break; + } + } + else if (allSubFeatures[i].type == LINE_FEATURE_L_JUMP_H2L) + { + //ڵLINE_FEATURE_L_SLOPE_L2HЧ + for (int j = i - 1; j >= 0; j--) + { + if (allSubFeatures[j].type == LINE_FEATURE_L_SLOPE_H2L) + allSubFeatures[j].type = LINE_FEATURE_UNDEF; + else + break; + } + } + } +#if 0 + std::vector< _FeatureInfo> subFeatures; + for (int i = 0; i < sampleNum; i++) + { + if (allSubFeatures[i].type != LINE_FEATURE_UNDEF) + subFeatures.push_back(allSubFeatures[i]); + } + _FeatureInfo a_nullFeature; + memset(&a_nullFeature, 0, sizeof(_FeatureInfo)); + subFeatures.push_back(a_nullFeature); //ڽβһգдʱȷһ + int subfeatureSize = subFeatures.size(); +#endif + //ϸsubFeatureѡ + int pre_type = LINE_FEATURE_UNDEF; + _FeatureInfo a_vldFeature; + std::vector< _FeatureInfo> vldFeatures; + for (int i = 0; i < sampleNum; i++) + { + if (pre_type == LINE_FEATURE_UNDEF) + { + if ( (allSubFeatures[i].type == LINE_FEATURE_L_JUMP_H2L) || + (allSubFeatures[i].type == LINE_FEATURE_L_JUMP_L2H)) + { + vldFeatures.push_back(allSubFeatures[i]); + pre_type = LINE_FEATURE_UNDEF; + } + else + { + a_vldFeature = allSubFeatures[i]; + pre_type = allSubFeatures[i].type; + } + } + else + { + if (allSubFeatures[i].type == LINE_FEATURE_UNDEF) + { + vldFeatures.push_back(a_vldFeature); + } + else + { + if (((a_vldFeature.k < 0) && (allSubFeatures[i].k < 0)) || + ((a_vldFeature.k > 0) && (allSubFeatures[i].k > 0))) + { + if (abs(a_vldFeature.k) < abs(allSubFeatures[i].k)) + a_vldFeature = allSubFeatures[i]; + } + else + { + vldFeatures.push_back(a_vldFeature); + a_vldFeature = allSubFeatures[i]; + } + } + pre_type = allSubFeatures[i].type; + } + } + + //ԣ + //ȶJump + std::vector featureBuffer; + featureBuffer.resize(vldFeatures.size()); + for (int i = 0; i < vldFeatures.size(); i++) //ʼ + featureBuffer[i].featureType = LINE_FEATURE_UNDEF; + for (int i = 0, i_max = vldFeatures.size(); i < i_max; i++) + { + if (vldFeatures[i].type == LINE_FEATURE_L_JUMP_H2L) + { + //жzķ + if (abs(vldFeatures[i].delta_z) < minPkHeighth) //ûƥJUMP + { + // + SSG_basicFeature1D a_feature; + a_feature.featureType = LINE_FEATURE_PEAK_TOP; + int centerIdx = (vldFeatures[i].sIdx + vldFeatures[i].eIdx) / 2; + a_feature.jumpPos2D = { lineIdx, centerIdx }; + a_feature.jumpPos.x = (vldFeatures[i].start.x + vldFeatures[i].end.x) / 2; + a_feature.jumpPos.y = (vldFeatures[i].start.y + vldFeatures[i].end.y) / 2; + a_feature.jumpPos.z = (vldFeatures[i].start.z + vldFeatures[i].end.z) / 2; + int start = vldFeatures[i].sIdx; + int end = vldFeatures[i].eIdx; + //ƥJUMP + if (i < i_max - 1) + { + if ((vldFeatures[i + 1].type == LINE_FEATURE_L_JUMP_H2L) || + (vldFeatures[i + 1].type == LINE_FEATURE_L_JUMP_L2H)) + { + //жϾ + double dist = abs(vldFeatures[i + 1].start.y - vldFeatures[i].end.y); + if (dist < holeR) //ϲ + { + centerIdx = (vldFeatures[i].sIdx + vldFeatures[i+1].eIdx) / 2; + a_feature.jumpPos2D = { lineIdx, centerIdx }; + a_feature.jumpPos.x = (vldFeatures[i].start.x + vldFeatures[i+1].end.x) / 2; + a_feature.jumpPos.y = (vldFeatures[i].start.y + vldFeatures[i+1].end.y) / 2; + a_feature.jumpPos.z = (vldFeatures[i].start.z + vldFeatures[i+1].end.z) / 2; + vldFeatures[i + 1].type = LINE_FEATURE_UNDEF; + end = vldFeatures[i+1].eIdx; + } + } + } + featureBuffer[i] = a_feature; + lineData[start].nPointIdx = LINE_FEATURE_NUM; //յʶʹLINE_FEATURE_NUMʶ + lineData[end].nPointIdx = LINE_FEATURE_NUM; + } + else //½ֻƥûJUMP + { + // + SSG_basicFeature1D a_feature; + a_feature.featureType = LINE_FEATURE_PEAK_TOP; + int centerIdx = (vldFeatures[i].sIdx + vldFeatures[i].eIdx) / 2; + a_feature.jumpPos2D = { lineIdx, centerIdx }; + a_feature.jumpPos.x = (vldFeatures[i].start.x + vldFeatures[i].end.x) / 2; + a_feature.jumpPos.y = (vldFeatures[i].start.y + vldFeatures[i].end.y) / 2; + a_feature.jumpPos.z = (vldFeatures[i].start.z + vldFeatures[i].end.z) / 2; + int start = vldFeatures[i].sIdx; + int end = vldFeatures[i].eIdx; + if (i < i_max - 1) + { + if ((vldFeatures[i + 1].type == LINE_FEATURE_L_JUMP_H2L) || + (vldFeatures[i + 1].type == LINE_FEATURE_L_JUMP_L2H)) + { + if (abs(vldFeatures[i + 1].delta_z) < minPkHeighth) //ֻƥû + { + //жϾ + double dist = abs(vldFeatures[i + 1].start.y - vldFeatures[i].end.y); + if (dist < holeR) //ϲ + { + centerIdx = (vldFeatures[i].sIdx + vldFeatures[i + 1].eIdx) / 2; + a_feature.jumpPos2D = { lineIdx, centerIdx }; + a_feature.jumpPos.x = (vldFeatures[i].start.x + vldFeatures[i + 1].end.x) / 2; + a_feature.jumpPos.y = (vldFeatures[i].start.y + vldFeatures[i + 1].end.y) / 2; + a_feature.jumpPos.z = (vldFeatures[i].start.z + vldFeatures[i + 1].end.z) / 2; + vldFeatures[i + 1].type = LINE_FEATURE_UNDEF; + end = vldFeatures[i + 1].eIdx; + } + } + } + } + featureBuffer[i] = a_feature; + lineData[start].nPointIdx = LINE_FEATURE_NUM; //յʶʹLINE_FEATURE_NUMʶ + lineData[end].nPointIdx = LINE_FEATURE_NUM; + } + } + else if (vldFeatures[i].type == LINE_FEATURE_L_JUMP_L2H) + { + //жzķ + if (abs(vldFeatures[i].delta_z) < minPkHeighth) //ûƥJUMP + { + //ֱƥJUMP + // + SSG_basicFeature1D a_feature; + a_feature.featureType = LINE_FEATURE_PEAK_TOP; + int centerIdx = (vldFeatures[i].sIdx + vldFeatures[i].eIdx) / 2; + a_feature.jumpPos2D = { lineIdx, centerIdx }; + a_feature.jumpPos.x = (vldFeatures[i].start.x + vldFeatures[i].end.x) / 2; + a_feature.jumpPos.y = (vldFeatures[i].start.y + vldFeatures[i].end.y) / 2; + a_feature.jumpPos.z = (vldFeatures[i].start.z + vldFeatures[i].end.z) / 2; + int start = vldFeatures[i].sIdx; + int end = vldFeatures[i].eIdx; + //ƥJUMP + if (i < i_max - 1) + { + if ((vldFeatures[i + 1].type == LINE_FEATURE_L_JUMP_H2L) || + (vldFeatures[i + 1].type == LINE_FEATURE_L_JUMP_L2H)) + { + //жϾ + double dist = abs(vldFeatures[i + 1].start.y - vldFeatures[i].end.y); + if (dist < holeR) //ϲ + { + centerIdx = (vldFeatures[i].sIdx + vldFeatures[i + 1].eIdx) / 2; + a_feature.jumpPos2D = { lineIdx, centerIdx }; + a_feature.jumpPos.x = (vldFeatures[i].start.x + vldFeatures[i + 1].end.x) / 2; + a_feature.jumpPos.y = (vldFeatures[i].start.y + vldFeatures[i + 1].end.y) / 2; + a_feature.jumpPos.z = (vldFeatures[i].start.z + vldFeatures[i + 1].end.z) / 2; + vldFeatures[i + 1].type = LINE_FEATURE_UNDEF; + end = vldFeatures[i + 1].eIdx; + } + } + } + featureBuffer[i] = a_feature; + lineData[start].nPointIdx = LINE_FEATURE_NUM; //յʶʹLINE_FEATURE_NUMʶ + lineData[end].nPointIdx = LINE_FEATURE_NUM; + } + else //ģƥ½JUMPûJUMPSLOPE + { + // + SSG_basicFeature1D a_feature; + a_feature.featureType = LINE_FEATURE_PEAK_TOP; + int centerIdx = (vldFeatures[i].sIdx + vldFeatures[i].eIdx) / 2; + a_feature.jumpPos2D = { lineIdx, centerIdx }; + a_feature.jumpPos.x = (vldFeatures[i].start.x + vldFeatures[i].end.x) / 2; + a_feature.jumpPos.y = (vldFeatures[i].start.y + vldFeatures[i].end.y) / 2; + a_feature.jumpPos.z = (vldFeatures[i].start.z + vldFeatures[i].end.z) / 2; + int start = vldFeatures[i].sIdx; + int end = vldFeatures[i].eIdx; + if (i < i_max - 1) + { + if ((vldFeatures[i + 1].type == LINE_FEATURE_L_JUMP_H2L) || + ((vldFeatures[i + 1].type == LINE_FEATURE_L_JUMP_L2H) && (abs(vldFeatures[i+1].delta_z) < minPkHeighth)) || + (vldFeatures[i + 1].type == LINE_FEATURE_L_SLOPE_H2L)) + { + //жϾ + double dist = abs(vldFeatures[i + 1].start.y - vldFeatures[i].end.y); + if (dist < holeR) //ϲ + { + centerIdx = (vldFeatures[i].sIdx + vldFeatures[i + 1].eIdx) / 2; + a_feature.jumpPos2D = { lineIdx, centerIdx }; + a_feature.jumpPos.x = (vldFeatures[i].start.x + vldFeatures[i + 1].end.x) / 2; + a_feature.jumpPos.y = (vldFeatures[i].start.y + vldFeatures[i + 1].end.y) / 2; + a_feature.jumpPos.z = (vldFeatures[i].start.z + vldFeatures[i + 1].end.z) / 2; + vldFeatures[i + 1].type = LINE_FEATURE_UNDEF; + end = vldFeatures[i + 1].eIdx; + } + + } + } + featureBuffer[i] = a_feature; + lineData[start].nPointIdx = LINE_FEATURE_NUM; //յʶʹLINE_FEATURE_NUMʶ + lineData[end].nPointIdx = LINE_FEATURE_NUM; + } + } + } + //SLOPE + for (int i = 0, i_max = vldFeatures.size(); i < i_max; i++) + { + if (vldFeatures[i].type == LINE_FEATURE_L_SLOPE_L2H) + { + if (i < i_max - 1) + { + double dist = abs(vldFeatures[i + 1].start.y - vldFeatures[i].end.y); + if ((dist < holeR) && (vldFeatures[i + 1].type == LINE_FEATURE_L_SLOPE_H2L)) + { + // + SSG_basicFeature1D a_feature; + a_feature.featureType = LINE_FEATURE_PEAK_TOP; + int centerIdx = (vldFeatures[i].sIdx + vldFeatures[i + 1].eIdx) / 2; + a_feature.jumpPos2D = { lineIdx, centerIdx }; + a_feature.jumpPos.x = (vldFeatures[i].start.x + vldFeatures[i + 1].end.x) / 2; + a_feature.jumpPos.y = (vldFeatures[i].start.y + vldFeatures[i + 1].end.y) / 2; + a_feature.jumpPos.z = (vldFeatures[i].start.z + vldFeatures[i + 1].end.z) / 2; + vldFeatures[i + 1].type = LINE_FEATURE_UNDEF; + featureBuffer[i] = a_feature; + } + else + { + SSG_basicFeature1D a_feature; + a_feature.featureType = LINE_FEATURE_PEAK_TOP; +#if 0 + int centerIdx = (vldFeatures[i].sIdx + vldFeatures[i].eIdx) / 2; + a_feature.jumpPos2D = { lineIdx, centerIdx }; + a_feature.jumpPos.x = (vldFeatures[i].start.x + vldFeatures[i].end.x) / 2; + a_feature.jumpPos.y = (vldFeatures[i].start.y + vldFeatures[i].end.y) / 2; + a_feature.jumpPos.z = (vldFeatures[i].start.z + vldFeatures[i].end.z) / 2; +#endif + //HoleRΧѰPeak + double mean_z = -1; + bool foundJump = false; + int pkIdx = _findSegMaxZPt(lineData, vldFeatures[i].eIdx, false, holeR*1.5, &mean_z, &foundJump); + //ѰǷJump + double meanZ_inv = -1; + bool foundJump_inv = false; + int pkIdx_inv = _findSegMaxZPt(lineData, vldFeatures[i].sIdx, true, holeR * 1.5, &meanZ_inv, &foundJump_inv); + if ( (pkIdx >= 0)&&(false == foundJump_inv)) + { + double mean_depth = mean_z - vldFeatures[i].start.z; + if (pkIdx_inv >= 0) + mean_depth = mean_z - meanZ_inv; + if (mean_depth >= slopeZDeltaTH/2) + { + a_feature.jumpPos2D = { lineIdx, pkIdx }; + a_feature.jumpPos = lineData[pkIdx].pt3D; + featureBuffer[i] = a_feature; + } + } + + } + } + } + else if (vldFeatures[i].type == LINE_FEATURE_L_SLOPE_H2L) + { + //HoleRΧѰPeak + SSG_basicFeature1D a_feature; + a_feature.featureType = LINE_FEATURE_PEAK_TOP; +#if 0 + int centerIdx = (vldFeatures[i].sIdx + vldFeatures[i].eIdx) / 2; + a_feature.jumpPos2D = { lineIdx, centerIdx }; + a_feature.jumpPos.x = (vldFeatures[i].start.x + vldFeatures[i].end.x) / 2; + a_feature.jumpPos.y = (vldFeatures[i].start.y + vldFeatures[i].end.y) / 2; + a_feature.jumpPos.z = (vldFeatures[i].start.z + vldFeatures[i].end.z) / 2; +#endif + //HoleRΧѰPeak + double mean_z = -1; + bool foundJump = false; + int pkIdx = _findSegMaxZPt(lineData, vldFeatures[i].sIdx, true, holeR*1.5, &mean_z, &foundJump); + //ѰǷJump + double meanZ_inv = -1; + bool foundJump_inv = false; + int pkIdx_inv = _findSegMaxZPt(lineData, vldFeatures[i].eIdx, false, holeR * 1.5, &meanZ_inv, &foundJump_inv); + if ((pkIdx >= 0) && (false == foundJump_inv)) + { + double mean_depth = mean_z - vldFeatures[i].start.z; + if (pkIdx_inv >= 0) + mean_depth = mean_z - meanZ_inv; + if (mean_depth >= slopeZDeltaTH/2) + { + a_feature.jumpPos2D = { lineIdx, pkIdx }; + a_feature.jumpPos = lineData[pkIdx].pt3D; + featureBuffer[i] = a_feature; + } + } + } + + } + for (int i = 0, i_max = vldFeatures.size(); i < i_max; i++) + { + if (featureBuffer[i].featureType != LINE_FEATURE_UNDEF) + { + localMax.push_back(featureBuffer[i]); + if (lineData[featureBuffer[i].jumpPos2D.y].pt3D.z < 1e-4) + lineData[featureBuffer[i].jumpPos2D.y].pt3D = featureBuffer[i].jumpPos; + lineData[featureBuffer[i].jumpPos2D.y].nPointIdx = LINE_FEATURE_PEAK_TOP; //nPointIdxתʹ + } + } + return; +} +#endif + /// /// ȡϵ㣨Zֵ䣩 /// From 724a5a15e66f5009e4a3d61fa6e9a43bba5cbb81 Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Wed, 16 Jul 2025 19:58:32 +0800 Subject: [PATCH 4/4] =?UTF-8?q?=E4=BF=AE=E6=94=B9=E6=9C=9D=E5=90=91?= =?UTF-8?q?=E6=98=BE=E7=A4=BA?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- bagPositioning_test/bagPositioning_test.cpp | 40 ++++++++++++++++----- 1 file changed, 31 insertions(+), 9 deletions(-) diff --git a/bagPositioning_test/bagPositioning_test.cpp b/bagPositioning_test/bagPositioning_test.cpp index 5c77ba2..d785b19 100644 --- a/bagPositioning_test/bagPositioning_test.cpp +++ b/bagPositioning_test/bagPositioning_test.cpp @@ -1660,20 +1660,42 @@ void _XOYprojection_RGBD( const double deg2rad = PI / 180.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 - x_range.min) / x_scale + x_skip); - int py1 = (int)((y1 - y_range.min) / y_scale + y_skip); - int px2 = (int)((x2 - x_range.min) / x_scale + x_skip); - int py2 = (int)((y2 - y_range.min) / y_scale + y_skip); + double arrowLen = R/3; + double arrowAngle = 30; + double ca = cos(arrowAngle * deg2rad); + double sa = sin(arrowAngle * deg2rad); + SVzNL2DPointD endingPt[4]; + endingPt[0] = { R, 0 }; + endingPt[1] = { -R, 0 }; + endingPt[2] = { R - arrowLen * ca, -arrowLen * sa }; + endingPt[3] = { R - arrowLen * ca, arrowLen * sa }; + for (int m = 0; m < 4; m++) + { + double tmp_x = endingPt[m].x * cy - endingPt[m].y * sy; + double tmp_y = -endingPt[m].x * sy - endingPt[m].y * cy; + endingPt[m].x = tmp_x + objOps[i].centerPos.x; + endingPt[m].y = tmp_y + objOps[i].centerPos.y; + } + int px1 = (int)((endingPt[0].x - x_range.min) / x_scale + x_skip); + int py1 = (int)((endingPt[0].y - y_range.min) / y_scale + y_skip); + int px2 = (int)((endingPt[1].x - x_range.min) / x_scale + x_skip); + int py2 = (int)((endingPt[1].y - y_range.min) / y_scale + y_skip); cv::line(img, cv::Point(px1, py1), cv::Point(px2, py2), cv::Scalar(rgb[2], rgb[1], rgb[0]), 2); + int px3 = (int)((endingPt[2].x - x_range.min) / x_scale + x_skip); + int py3 = (int)((endingPt[2].y - y_range.min) / y_scale + y_skip); + int px4 = (int)((endingPt[3].x - x_range.min) / x_scale + x_skip); + int py4 = (int)((endingPt[3].y - y_range.min) / y_scale + y_skip); if (objOps[i].orienFlag == 1) { - cv::circle(img, cv::Point(px1, py1), 5, cv::Scalar(0, 255, 0), -1); + cv::line(img, cv::Point(px1, py1), cv::Point(px3, py3), cv::Scalar(0, 255, 0), 2); + cv::line(img, cv::Point(px1, py1), cv::Point(px4, py4), cv::Scalar(0, 255, 0), 2); + //cv::circle(img, cv::Point(px1, py1), 5, cv::Scalar(0, 255, 0), -1); } else if (objOps[i].orienFlag == 2) { - cv::circle(img, cv::Point(px1, py1), 5, cv::Scalar(0, 0, 255), -1); + cv::line(img, cv::Point(px1, py1), cv::Point(px3, py3), cv::Scalar(0, 0, 255), 2); + cv::line(img, cv::Point(px1, py1), cv::Point(px4, py4), cv::Scalar(0, 0, 255), 2); + //cv::circle(img, cv::Point(px1, py1), 5, cv::Scalar(0, 0, 255), -1); } } } @@ -2682,7 +2704,7 @@ int main() } for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) { - fidx = 10; + //fidx = 10; if (grp < TEST_TOP_VIEW_GROUP) //正面抓取 { int lineNum = 0;