From e18e46a43c3c5fcbc7280a6b748efbd20858341c Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Mon, 10 Nov 2025 23:24:06 +0800 Subject: [PATCH] =?UTF-8?q?=E5=90=88=E5=B9=B6=20=E7=B2=92=E5=BE=84?= =?UTF-8?q?=E7=AE=97=E6=B3=95=20ver=5F1.0.0=20=E4=BF=AE=E6=94=B9=E5=AF=BC?= =?UTF-8?q?=E5=87=BA=E7=AC=A6=E5=8F=B7=E5=B9=B6=E7=BC=96=E8=AF=91Arm?= =?UTF-8?q?=E7=89=88=E6=9C=AC?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../BQ_workpieceCornerExtract_test.cpp | 27 +- CMakeLists.txt | 2 + .../particleSizeMeasurement.vcxproj | 172 ++++ .../particleSizeMeasurement_test.cpp | 632 +++++++++++++++ .../particleSizeMeasurement_test.vcxproj | 159 ++++ .../BQ_workpieceCornerExtraction_Export.h | 24 +- sourceCode/SG_bagPositioning_Export.h | 31 +- sourceCode/SG_baseAlgo_Export.h | 24 +- sourceCode/SG_regionGrow.cpp | 2 +- sourceCode/SX_lapWeldDetection_Export.h | 22 +- sourceCode/WD_particleSizeMeasure.cpp | 741 +++--------------- sourceCode/WD_particleSizeMeasure_Export.h | 17 +- sourceCode/WD_watershed.cpp | 166 +++- 13 files changed, 1312 insertions(+), 707 deletions(-) create mode 100644 particleSizeMeasurement/particleSizeMeasurement.vcxproj create mode 100644 particleSizeMeasurement_test/particleSizeMeasurement_test.cpp create mode 100644 particleSizeMeasurement_test/particleSizeMeasurement_test.vcxproj diff --git a/BQ_workpieceCornerExtract_test/BQ_workpieceCornerExtract_test.cpp b/BQ_workpieceCornerExtract_test/BQ_workpieceCornerExtract_test.cpp index fc494f1..3b7f34c 100644 --- a/BQ_workpieceCornerExtract_test/BQ_workpieceCornerExtract_test.cpp +++ b/BQ_workpieceCornerExtract_test/BQ_workpieceCornerExtract_test.cpp @@ -81,6 +81,27 @@ void vzReadLaserScanPointFromFile_XYZ_vector(const char* fileName, std::vector> scanLines) +{ + std::ofstream sw(fileName); + int lineNum = scanLines.size(); + for (int line = 0; line < lineNum; line++) + { + int nPositionCnt = scanLines[line].size(); + for (int i = 0; i < nPositionCnt; i++) + { + SVzNL3DPoint* pt3D = &scanLines[line][i].pt3D; + if (pt3D->z < 1e-4) + continue; + double x = (double)pt3D->x; + double y = (double)pt3D->y; + double z = (double)pt3D->z; + sw << x << "," << y << "," << z << std::endl; + } + } + sw.close(); +} + void _convertToGridData_XYZ_vector(std::vector>& scanData, double _F, std::vector>& scanData_grid) { int min_y = 100000000; @@ -619,7 +640,7 @@ int main() }; SVzNLRange fileIdx[TEST_GROUP] = { - {1,3}, {6,8}, {6,8}, {6,8}, {6,8}, {9,12} + {1,3}, {6,8}, {6,8}, {6,8}, {6,8}, {9,11} }; #if CONVERT_TO_GRID @@ -755,6 +776,10 @@ int main() std::vector> scanLines; vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines); + //转成plyTxt格式 + //sprintf_s(_scan_file, "%s%d_ply_Hi229229.txt", dataPath[grp], fidx); + //wdSavePlyTxt(_scan_file, scanLines); + long t1 = (long)GetTickCount64();//统计时间 for (int i = 0, i_max = (int)scanLines.size(); i < i_max; i++) diff --git a/CMakeLists.txt b/CMakeLists.txt index cbfcbb1..1da4f28 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -42,6 +42,8 @@ ADD_LIBRARY(beltTearingDetection SHARED sourceCode/beltTearingDetection.cpp) ADD_LIBRARY(workpieceCornerExtraction SHARED sourceCode/BQ_workpieceCornerExtraction.cpp) +ADD_LIBRARY(particleSizeMeasurement SHARED sourceCode/WD_particleSizeMeasure.cpp) + #add executable file # ADD_EXECUTABLE(bagPositioning_test bagPositioning_test/bagPositioning_test.cpp) diff --git a/particleSizeMeasurement/particleSizeMeasurement.vcxproj b/particleSizeMeasurement/particleSizeMeasurement.vcxproj new file mode 100644 index 0000000..82137c0 --- /dev/null +++ b/particleSizeMeasurement/particleSizeMeasurement.vcxproj @@ -0,0 +1,172 @@ + + + + + Debug + Win32 + + + Release + Win32 + + + Debug + x64 + + + Release + x64 + + + + + + + + + + 16.0 + Win32Proj + {8fd0f574-61c6-4094-8521-33aeddb44797} + particleSizeMeasurement + 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;..\..\thirdParty\opencv320\build\include;..\sourceCode;..\sourceCode\inc;$(IncludePath) + + + false + $(SolutionDir)build\$(Platform)\$(Configuration)\ + ..\..\thirdParty\VzNLSDK\Inc;..\..\thirdParty\opencv320\build\include;..\sourceCode;..\sourceCode\inc;$(IncludePath) + + + + Level3 + true + WIN32;_DEBUG;PARTICLESIZEMEASUREMENT_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) + true + Use + pch.h + + + Windows + true + false + + + + + Level3 + true + true + true + WIN32;NDEBUG;PARTICLESIZEMEASUREMENT_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) + true + Use + pch.h + + + Windows + true + true + true + false + + + + + Level3 + true + _DEBUG;PARTICLESIZEMEASUREMENT_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) + true + NotUsing + pch.h + /D_CRT_SECURE_NO_WARNINGS %(AdditionalOptions) + + + Windows + true + false + ..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Debug;%(AdditionalLibraryDirectories) + opencv_world320d.lib;baseAlgorithm.lib;%(AdditionalDependencies) + + + + + Level3 + true + true + true + NDEBUG;PARTICLESIZEMEASUREMENT_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) + true + NotUsing + pch.h + /D_CRT_SECURE_NO_WARNINGS %(AdditionalOptions) + + + Windows + true + true + true + false + ..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Release;%(AdditionalLibraryDirectories) + opencv_world320.lib;baseAlgorithm.lib;%(AdditionalDependencies) + + + + + + \ No newline at end of file diff --git a/particleSizeMeasurement_test/particleSizeMeasurement_test.cpp b/particleSizeMeasurement_test/particleSizeMeasurement_test.cpp new file mode 100644 index 0000000..47275c3 --- /dev/null +++ b/particleSizeMeasurement_test/particleSizeMeasurement_test.cpp @@ -0,0 +1,632 @@ +// BQ_workpieceCornerExtract_test.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。 +// + +#include +#include +#include +#include +#include +#include "direct.h" +#include +#include "WD_particleSizeMeasure_Export.h" +#include +#include +#include + +typedef struct +{ + int r; + int g; + int b; +}SG_color; + +typedef struct +{ + int nPointIdx; + double x; + double y; + double z; + float r; + float g; + float b; +} SPointXYZRGB; + +void vzReadLaserScanPointFromFile_XYZ_vector(const char* fileName, std::vector>& scanData) +{ + std::ifstream inputFile(fileName); + std::string linedata; + + if (inputFile.is_open() == false) + return; + + std::vector< SVzNL3DPosition> a_line; + int ptIdx = 0; + while (getline(inputFile, linedata)) + { + if (0 == strncmp("Line_", linedata.c_str(), 5)) + { + int ptSize = (int)a_line.size(); + if (ptSize > 0) + { + scanData.push_back(a_line); + } + a_line.clear(); + ptIdx = 0; + } + 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); + SVzNL3DPosition a_pt; + a_pt.pt3D.x = X; + a_pt.pt3D.y = Y; + a_pt.pt3D.z = Z; + a_pt.nPointIdx = ptIdx; + ptIdx++; + a_line.push_back(a_pt); + } + } + //last line + int ptSize = (int)a_line.size(); + if (ptSize > 0) + { + scanData.push_back(a_line); + a_line.clear(); + } + + inputFile.close(); + return; +} + +void _convertToGridData_XYZ_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< SVzNL3DPosition>& a_line = scanData[line]; + int nPointCnt = a_line.size(); + for (int i = 0; i < nPointCnt; i++) + { + SVzNL3DPosition* a_pt = &scanData[line][i]; + if (a_pt->pt3D.z > 1e-4) + { + double v = _F * a_pt->pt3D.y / a_pt->pt3D.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; + for (int line = 0; line < lineNum; line++) + { + std::vector< SVzNL3DPosition> gridData; + gridData.resize(pt_counter); + for (int i = 0; i < pt_counter; i++) + gridData[i] = { 0,{ 0.0, 0.0, 0.0} }; + + std::vector< SVzNL3DPosition>& a_line = scanData[line]; + int nPointCnt = a_line.size(); + for (int i = 0; i < nPointCnt; i++) + { + SVzNL3DPosition a_pt = a_line[i]; + if (a_pt.pt3D.z > 1e-4) + { + int pt_id = a_pt.nPointIdx - min_y; + gridData[pt_id] = a_pt; + } + } + scanData_grid.push_back(gridData); + } + return; +} + +void _outputScanDataFile_XYZ_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++) + { + SVzNL3DPosition* pt3D = &scanData[line][i]; + float x = (float)pt3D->pt3D.x; + float y = (float)pt3D->pt3D.y; + float z = (float)pt3D->pt3D.z; + char str[250]; + sprintf_s(str, "{ %f, %f, %f } - { 0, 0 } - { 0, 0 }", x, y, z); + + sw << str << std::endl; + } + } + sw.close(); +} + +void _getRoiData_XYZ_vector( + std::vector>& scanData, + std::vector>& roiData, + SVzNL3DRangeD roi) +{ + int lineNum = scanData.size(); + for (int line = 0; line < lineNum; line++) + { + int nPositionCnt = scanData[line].size(); + std::vector< SVzNL3DPosition> linePts; + for (int i = 0; i < nPositionCnt; i++) + { + SVzNL3DPosition pt3D = scanData[line][i]; + if ((pt3D.pt3D.z >= roi.zRange.min) && + (pt3D.pt3D.z <= roi.zRange.max) && + (pt3D.pt3D.y >= roi.yRange.min) && + (pt3D.pt3D.y <= roi.yRange.max)) + { + linePts.push_back(pt3D); + } + } + roiData.push_back(linePts); + } +} + +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(); +} + +void _outputParticleSizeInfo(char* fileName, std::vector& particles) +{ + int particleSize = (int)particles.size(); + if (particleSize == 0) + return; + + std::ofstream sw(fileName); + char dataStr[250]; + sprintf_s(dataStr, 250, "particleNum: %d", particleSize); + sw << dataStr << std::endl; + + for (int i = 0; i < particleSize; i++) + { + sprintf_s(dataStr, 250, " id_%d: (%g, %g, %g)", i, particles[i].size.length, particles[i].size.width, particles[i].size.height); + sw << dataStr << std::endl; + } + sw.close(); +} + +void _outputScanDataFile_vector(char* fileName, std::vector>& scanLines, bool removeZeros, int* headNullLines) +{ + std::ofstream sw(fileName); + int lineNum = (int)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 = (int)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(); +} + +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 _outputScanDataResult_RGBD( + char* fileName, + std::vector>& scanLines, + std::vector& particles) +{ + int lineNum = (int)scanLines.size(); + std::ofstream sw(fileName); + int realLines = lineNum; + if (particles.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 }; + int size = 1; + int lineIdx = 0; + for (int line = 0; line < lineNum; line++) + { + int linePtNum = (int)scanLines[line].size(); + if (linePtNum == 0) + continue; + + int vldNum = 0; + for (int i = 0; i < linePtNum; i++) + { + if (scanLines[line][i].pt3D.z > 1e-4) + vldNum++; + } + + sw << "Line_" << lineIdx << "_0_" << vldNum << std::endl; + lineIdx++; + for (int i = 0; i < linePtNum; i++) + { + if (scanLines[line][i].pt3D.z <= 1e-4) + continue; + + SVzNL3DPosition* pt3D = &scanLines[line][i]; + int mkID = pt3D->nPointIdx; + if (mkID < 2) + { + rgb = { 200, 200, 200 }; + size = 1; + } + else + { + rgb.r = (mkID * 97) % 256; + rgb.g = (mkID * 73) % 256; + rgb.b = (mkID * 59) % 256; + 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; + } + } + + rgb = { 255,0,0 }; + if (particles.size() > 0) + { + int objNum = (int)particles.size(); + int lineIdx = 0; + for (int i = 0; i < objNum; i++) + { + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << particles[i].vertix[0].x << "," << particles[i].vertix[0].y << "," << particles[i].vertix[0].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << particles[i].vertix[1].x << "," << particles[i].vertix[1].y << "," << particles[i].vertix[1].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + lineIdx++; + + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << particles[i].vertix[1].x << "," << particles[i].vertix[1].y << "," << particles[i].vertix[1].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << particles[i].vertix[2].x << "," << particles[i].vertix[2].y << "," << particles[i].vertix[2].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + lineIdx++; + + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << particles[i].vertix[2].x << "," << particles[i].vertix[2].y << "," << particles[i].vertix[2].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << particles[i].vertix[3].x << "," << particles[i].vertix[3].y << "," << particles[i].vertix[3].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + lineIdx++; + + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << particles[i].vertix[3].x << "," << particles[i].vertix[3].y << "," << particles[i].vertix[3].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << particles[i].vertix[0].x << "," << particles[i].vertix[0].y << "," << particles[i].vertix[0].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + lineIdx++; + + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << particles[i].vertix[0].x << "," << particles[i].vertix[0].y << "," << particles[i].vertix[0].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << particles[i].vertix[4].x << "," << particles[i].vertix[4].y << "," << particles[i].vertix[4].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + lineIdx++; + + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << particles[i].vertix[1].x << "," << particles[i].vertix[1].y << "," << particles[i].vertix[1].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << particles[i].vertix[5].x << "," << particles[i].vertix[5].y << "," << particles[i].vertix[5].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + lineIdx++; + + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << particles[i].vertix[2].x << "," << particles[i].vertix[2].y << "," << particles[i].vertix[2].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << particles[i].vertix[6].x << "," << particles[i].vertix[6].y << "," << particles[i].vertix[6].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + lineIdx++; + + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << particles[i].vertix[3].x << "," << particles[i].vertix[3].y << "," << particles[i].vertix[3].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << particles[i].vertix[7].x << "," << particles[i].vertix[7].y << "," << particles[i].vertix[7].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + lineIdx++; + + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << particles[i].vertix[4].x << "," << particles[i].vertix[4].y << "," << particles[i].vertix[4].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << particles[i].vertix[5].x << "," << particles[i].vertix[5].y << "," << particles[i].vertix[5].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + lineIdx++; + + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << particles[i].vertix[5].x << "," << particles[i].vertix[5].y << "," << particles[i].vertix[5].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << particles[i].vertix[6].x << "," << particles[i].vertix[6].y << "," << particles[i].vertix[6].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + lineIdx++; + + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << particles[i].vertix[6].x << "," << particles[i].vertix[6].y << "," << particles[i].vertix[6].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << particles[i].vertix[7].x << "," << particles[i].vertix[7].y << "," << particles[i].vertix[7].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + lineIdx++; + + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << particles[i].vertix[7].x << "," << particles[i].vertix[7].y << "," << particles[i].vertix[7].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << particles[i].vertix[4].x << "," << particles[i].vertix[4].y << "," << particles[i].vertix[4].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + lineIdx++; + } + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << particles[0].vertix[0].x << "," << particles[0].vertix[0].y << "," << particles[0].vertix[0].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << particles[0].vertix[1].x << "," << particles[0].vertix[1].y << "," << particles[0].vertix[1].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + lineIdx++; + } + + sw.close(); +} + +#define CONVERT_TO_GRID 0 +#define TEST_COMPUTE_CALIB_PARA 0 +#define TEST_COMPUTE_PARTICE_SIZE 1 +#define TEST_GROUP 1 +int main() +{ + const char* dataPath[TEST_GROUP] = { + "F:\\ShangGu\\粒径数据\\曝光\\3D数据\\" //0 + }; + + SVzNLRange fileIdx[TEST_GROUP] = { + {1,10} + }; + +#if TEST_COMPUTE_PARTICE_SIZE + for (int grp = 0; grp <= 0; grp++) + { + 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 = 2600.0; + for (int i = 0; i < 9; i++) + poseCalibPara.invRMatrix[i] = poseCalibPara.planeCalib[i]; + char calibFile[250]; +#if 0 + if (grp == 0) + { + sprintf_s(calibFile, "F:\\ShangGu\\粒径数据\\曝光\\3D数据\\ground_calib_para.txt"); + poseCalibPara = _readCalibPara(calibFile); + } +#endif + for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) + { + //fidx =1; + char _scan_file[256]; + sprintf_s(_scan_file, "%sgridScanData_%d.txt", dataPath[grp], fidx); + std::vector> scanLines; + vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines); + + long t1 = (long)GetTickCount64();//统计时间 + for (int i = 0, i_max = (int)scanLines.size(); i < i_max; i++) + { + if (i == 14) + int kkk = 1; + //行处理 + //调平，去除地面 + wd_lineDataR(scanLines[i], poseCalibPara.planeCalib, -1); + } + + SWD_paricleSizeParam paricleSizeParam; + paricleSizeParam.minSize = { 100.0, 100.0, 100.0 }; + paricleSizeParam.alarmSize = { 1000.0, 1000.0, 1000.0 }; + SWD_PSM_algoParam algoParam; + algoParam.filterParam.continuityTh = 20.0; //噪声滤除。当相邻点的z跳变大于此门限时，检查是否为噪声。若长度小于outlierLen， 视为噪声 + algoParam.filterParam.outlierTh = 5; + algoParam.cornerParam.cornerTh = 60; //45度角 + algoParam.cornerParam.scale = 50; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8; + algoParam.cornerParam.minEndingGap = 20; // algoParam.bagParam.bagW / 4; + algoParam.cornerParam.minEndingGap_z = 20; + algoParam.cornerParam.jumpCornerTh_1 = 10; //水平角度，小于此角度视为水平 + algoParam.cornerParam.jumpCornerTh_2 = 60; + algoParam.growParam.maxLineSkipNum = 10; + algoParam.growParam.yDeviation_max = 10.0; + algoParam.growParam.maxSkipDistance = 10.0; + algoParam.growParam.zDeviation_max = 10.0;// algoParam.bagParam.bagH / 2; //袋子高度1/2 + algoParam.growParam.minLTypeTreeLen = 100; //mm + algoParam.growParam.minVTypeTreeLen = 100; //mm + + int errCode = 0; + std::vector particles; + wd_particleSizeMeasure( + scanLines, + paricleSizeParam, + poseCalibPara, + algoParam, + particles, + &errCode); + long t2 = (long)GetTickCount64(); + printf("%s: %d(ms)!\n", _scan_file, (int)(t2 - t1)); + //输出测试结果 + sprintf_s(_scan_file, "%sresult\\LaserLine%d_result.txt", dataPath[grp], fidx); + _outputScanDataResult_RGBD(_scan_file, scanLines, particles); + sprintf_s(calibFile, "%sresult\\LaserLine%d_corner_info.txt", dataPath[grp], fidx); + _outputParticleSizeInfo(calibFile, particles); + } + } +#endif +} + +// 运行程序: Ctrl + F5 或调试 >“开始执行(不调试)”菜单 +// 调试程序: F5 或调试 >“开始调试”菜单 + +// 入门使用技巧: +// 1. 使用解决方案资源管理器窗口添加/管理文件 +// 2. 使用团队资源管理器窗口连接到源代码管理 +// 3. 使用输出窗口查看生成输出和其他消息 +// 4. 使用错误列表窗口查看错误 +// 5. 转到“项目”>“添加新项”以创建新的代码文件，或转到“项目”>“添加现有项”以将现有代码文件添加到项目 +// 6. 将来，若要再次打开此项目，请转到“文件”>“打开”>“项目”并选择 .sln 文件 + diff --git a/particleSizeMeasurement_test/particleSizeMeasurement_test.vcxproj b/particleSizeMeasurement_test/particleSizeMeasurement_test.vcxproj new file mode 100644 index 0000000..080e33b --- /dev/null +++ b/particleSizeMeasurement_test/particleSizeMeasurement_test.vcxproj @@ -0,0 +1,159 @@ + + + + + Debug + Win32 + + + Release + Win32 + + + Debug + x64 + + + Release + x64 + + + + 16.0 + Win32Proj + {0f4995a6-3978-4ef6-87ad-cc15ec9b007b} + particleSizeMeasurementtest + 10.0 + + + + Application + true + v142 + Unicode + + + Application + false + v142 + true + Unicode + + + Application + true + v142 + Unicode + + + Application + 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;..\sourceCode;..\sourceCode\inc;$(IncludePath) + + + + 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\opencv320\build\include; + /D_CRT_SECURE_NO_WARNINGS %(AdditionalOptions) + + + Console + true + opencv_world320d.lib;particleSizeMeasurement.lib;%(AdditionalDependencies) + ..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Debug;%(AdditionalLibraryDirectories) + + + + + Level3 + true + true + true + NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + ..\..\thirdParty\opencv320\build\include; + /D_CRT_SECURE_NO_WARNINGS %(AdditionalOptions) + + + Console + true + true + true + ..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Release;%(AdditionalLibraryDirectories) + opencv_world320.lib;particleSizeMeasurement.lib;%(AdditionalDependencies) + + + + + + + + + \ No newline at end of file diff --git a/sourceCode/BQ_workpieceCornerExtraction_Export.h b/sourceCode/BQ_workpieceCornerExtraction_Export.h index 2d2ddea..2147f0f 100644 --- a/sourceCode/BQ_workpieceCornerExtraction_Export.h +++ b/sourceCode/BQ_workpieceCornerExtraction_Export.h @@ -1,20 +1,6 @@ #pragma once -#if defined(_MSC_VER) || defined(WIN64) || defined(_WIN64) || defined(__WIN64__) || defined(WIN32) || defined(_WIN32) || defined(__WIN32__) || defined(__NT__) -# define Q_DECL_EXPORT __declspec(dllexport) -# define Q_DECL_IMPORT __declspec(dllimport) -#else -# define Q_DECL_EXPORT __attribute__((visibility("default"))) -# define Q_DECL_IMPORT __attribute__((visibility("default"))) -#endif - -#if defined(SG_API_LIBRARY) -# define SG_WORKPIECESHARED_EXPORT Q_DECL_EXPORT -#else -# define SG_WORKPIECESHARED_EXPORT Q_DECL_IMPORT - -#endif - +#include "SG_baseAlgo_Export.h" #include "SG_baseDataType.h" #include @@ -49,22 +35,22 @@ typedef struct }SSX_debugInfo; //汾 -SG_WORKPIECESHARED_EXPORT const char* wd_BQWorkpieceCornerVersion(void); +SG_APISHARED_EXPORT const char* wd_BQWorkpieceCornerVersion(void); //һƽƽ //пһƽͲοƽƽ棬ߵƽеƽ //תΪƽƽ淨ΪֱIJ -SG_WORKPIECESHARED_EXPORT SSG_planeCalibPara sx_BQ_getBaseCalibPara( +SG_APISHARED_EXPORT SSG_planeCalibPara sx_BQ_getBaseCalibPara( std::vector< std::vector>& scanLines); //̬ƽȥ -SG_WORKPIECESHARED_EXPORT void sx_BQ_lineDataR( +SG_APISHARED_EXPORT void sx_BQ_lineDataR( std::vector< SVzNL3DPosition>& a_line, const double* camPoseR, double groundH); //ȡӺ -SG_WORKPIECESHARED_EXPORT SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( +SG_APISHARED_EXPORT SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( std::vector< std::vector>& scanLines, const SSG_cornerParam cornerPara, const SSG_outlierFilterParam filterParam, diff --git a/sourceCode/SG_bagPositioning_Export.h b/sourceCode/SG_bagPositioning_Export.h index 44953e9..8843db6 100644 --- a/sourceCode/SG_bagPositioning_Export.h +++ b/sourceCode/SG_bagPositioning_Export.h @@ -1,23 +1,10 @@ #pragma once -#if defined(_MSC_VER) || defined(WIN64) || defined(_WIN64) || defined(__WIN64__) || defined(WIN32) || defined(_WIN32) || defined(__WIN32__) || defined(__NT__) -# define Q_DECL_EXPORT __declspec(dllexport) -# define Q_DECL_IMPORT __declspec(dllimport) -#else -# define Q_DECL_EXPORT __attribute__((visibility("default"))) -# define Q_DECL_IMPORT __attribute__((visibility("default"))) -#endif - -#if defined(SG_API_LIBRARY) -# define SG_BAGSHARED_EXPORT Q_DECL_EXPORT -#else -# define SG_BAGSHARED_EXPORT Q_DECL_IMPORT -#endif - #define OUTPUT_DEBUG 0 #define BAG_ALGO_USE_CORNER_FEATURE 1 #define RGN_HIST_SIZE 16 //ĿɫͳƵĿ +#include "SG_baseAlgo_Export.h" #include "SG_baseDataType.h" #include @@ -55,17 +42,17 @@ typedef struct }SSG_stackBaseParam; //ݵƽ -SG_BAGSHARED_EXPORT void sg_lineDataR( +SG_APISHARED_EXPORT void sg_lineDataR( SVzNL3DLaserLine* a_line, const double* camPoseR, double groundH); -SG_BAGSHARED_EXPORT void sg_lineDataR_RGBD( +SG_APISHARED_EXPORT void sg_lineDataR_RGBD( SVzNLXYZRGBDLaserLine* a_line, const double* camPoseR, double groundH); -SG_BAGSHARED_EXPORT void sg_bagPositioning_lineProc( +SG_APISHARED_EXPORT void sg_bagPositioning_lineProc( SVzNL3DLaserLine* a_line, int lineIdx, int* errCode, @@ -74,7 +61,7 @@ SG_BAGSHARED_EXPORT void sg_bagPositioning_lineProc( const SG_bagPositionParam algoParam); //ȡ֯ץȡ̬Ӷץȡ -SG_BAGSHARED_EXPORT void sg_getBagPosition( +SG_APISHARED_EXPORT void sg_getBagPosition( SVzNL3DLaserLine* laser3DPoints, int lineNum, //std::vector& all_vLineFeatures, @@ -83,7 +70,7 @@ SG_BAGSHARED_EXPORT void sg_getBagPosition( const SSG_planeCalibPara poseCalibPara, std::vector& objOps); -SG_BAGSHARED_EXPORT void sg_getBagPositionAndOrientation( +SG_APISHARED_EXPORT void sg_getBagPositionAndOrientation( SVzNLXYZRGBDLaserLine* laser3DPoints, int lineNum, //std::vector& all_vLineFeatures, @@ -100,13 +87,13 @@ SG_BAGSHARED_EXPORT void sg_getBagPositionAndOrientation( #endif int* errCode); -SG_BAGSHARED_EXPORT void sg_sideBagPosition( +SG_APISHARED_EXPORT void sg_sideBagPosition( SVzNL3DLaserLine* laser3DPoints, int lineNum, const SG_bagPositionParam algoParam, std::vector& objOps); -SG_BAGSHARED_EXPORT void sg_getSideBagStackBasePosition( +SG_APISHARED_EXPORT void sg_getSideBagStackBasePosition( SVzNL3DLaserLine* laser3DPoints, int lineNum, const SSG_stackBaseParam stackBaseParam, @@ -117,6 +104,6 @@ SG_BAGSHARED_EXPORT void sg_getSideBagStackBasePosition( //һƽƽ //пһƽͲοƽƽ棬ߵƽеƽ //תΪƽƽ淨ΪֱIJ -SG_BAGSHARED_EXPORT SSG_planeCalibPara sg_getBagBaseCalibPara( +SG_APISHARED_EXPORT SSG_planeCalibPara sg_getBagBaseCalibPara( SVzNL3DLaserLine* laser3DPoints, int lineNum); \ No newline at end of file diff --git a/sourceCode/SG_baseAlgo_Export.h b/sourceCode/SG_baseAlgo_Export.h index ba0d007..f253048 100644 --- a/sourceCode/SG_baseAlgo_Export.h +++ b/sourceCode/SG_baseAlgo_Export.h @@ -1,20 +1,19 @@ #pragma once - - #if defined(_MSC_VER) || defined(WIN64) || defined(_WIN64) || defined(__WIN64__) || defined(WIN32) || defined(_WIN32) || defined(__WIN32__) || defined(__NT__) -# define Q_ALGO_DECL_EXPORT __declspec(dllexport) -# define Q_ALGO_DECL_IMPORT __declspec(dllimport) +# define Q_DECL_EXPORT __declspec(dllexport) +# define Q_DECL_IMPORT __declspec(dllimport) #else -# define Q_ALGO_DECL_EXPORT __attribute__((visibility("default"))) -# define Q_ALGO_DECL_IMPORT __attribute__((visibility("default"))) +# define Q_DECL_EXPORT __attribute__((visibility("default"))) +# define Q_DECL_IMPORT __attribute__((visibility("default"))) #endif #if defined(SG_API_LIBRARY) -# define SG_APISHARED_EXPORT Q_ALGO_DECL_EXPORT +# define SG_APISHARED_EXPORT Q_DECL_EXPORT #else -# define SG_APISHARED_EXPORT Q_ALGO_DECL_IMPORT +# define SG_APISHARED_EXPORT Q_DECL_IMPORT #endif + #include "SG_baseDataType.h" #include #include @@ -422,4 +421,11 @@ SG_APISHARED_EXPORT void pointClout2DProjection( ); //ˮ㷨 -SG_APISHARED_EXPORT void watershed(SWD_waterShedImage& img); \ No newline at end of file +SG_APISHARED_EXPORT void watershed(SWD_waterShedImage& img); +// ӵзˮ㷨 +SG_APISHARED_EXPORT void wd_seedWatershed( + SWD_waterShedImage& img, + std::vector& watershedSeeds, //ӵ + int maxLevel, //ˮλ + int startMakerID //ʼMarkerID +); \ No newline at end of file diff --git a/sourceCode/SG_regionGrow.cpp b/sourceCode/SG_regionGrow.cpp index f6692ce..f184102 100644 --- a/sourceCode/SG_regionGrow.cpp +++ b/sourceCode/SG_regionGrow.cpp @@ -1339,7 +1339,7 @@ void sg_getLocalPeaks_distTransform(cv::Mat& input, std::vector& p for (int j = 0; j < winNum_cols; j++) { SVzNL2DPoint LTpos = { j * searchWin.searchW_pts / 2 , i * searchWin.seachW_lines / 2 }; - SSG_2DValueI pkPos = { -1, -1, 0, 0 }; + SSG_2DValueI pkPos = { -1, -1, 0, 0, 0}; _findDistTransformPeak(input, LTpos, searchWin, &pkPos); if ((pkPos.x >= 0) && (pkPos.y >= 0)) { diff --git a/sourceCode/SX_lapWeldDetection_Export.h b/sourceCode/SX_lapWeldDetection_Export.h index ce9893e..137ca8f 100644 --- a/sourceCode/SX_lapWeldDetection_Export.h +++ b/sourceCode/SX_lapWeldDetection_Export.h @@ -1,20 +1,6 @@ #pragma once -#if defined(_MSC_VER) || defined(WIN64) || defined(_WIN64) || defined(__WIN64__) || defined(WIN32) || defined(_WIN32) || defined(__WIN32__) || defined(__NT__) -# define Q_DECL_EXPORT __declspec(dllexport) -# define Q_DECL_IMPORT __declspec(dllimport) -#else -# define Q_DECL_EXPORT __attribute__((visibility("default"))) -# define Q_DECL_IMPORT __attribute__((visibility("default"))) -#endif - -#if defined(SG_API_LIBRARY) -# define SG_WELDSHARED_EXPORT Q_DECL_EXPORT -#else -# define SG_WELDSHARED_EXPORT Q_DECL_IMPORT -#endif - - +#include "SG_baseAlgo_Export.h" #include "SG_baseDataType.h" #include #include @@ -36,17 +22,17 @@ typedef struct //һƽƽ //пһƽͲοƽƽ棬ߵƽеƽ //תΪƽƽ淨ΪֱIJ -SG_WELDSHARED_EXPORT SSG_planeCalibPara sx_getBaseCalibPara( +SG_APISHARED_EXPORT SSG_planeCalibPara sx_getBaseCalibPara( std::vector< std::vector>& scanLines); //̬ƽȥ -SG_WELDSHARED_EXPORT void sx_lineDataR( +SG_APISHARED_EXPORT void sx_lineDataR( std::vector< SVzNL3DPosition>& a_line, const double* camPoseR, double groundH); //ȡӺ -SG_WELDSHARED_EXPORT void sx_getLapWeldPostion( +SG_APISHARED_EXPORT void sx_getLapWeldPostion( std::vector< std::vector>& scanLines, const SSG_cornerParam cornerPara, SSG_treeGrowParam growParam, diff --git a/sourceCode/WD_particleSizeMeasure.cpp b/sourceCode/WD_particleSizeMeasure.cpp index 747c5ba..53bd815 100644 --- a/sourceCode/WD_particleSizeMeasure.cpp +++ b/sourceCode/WD_particleSizeMeasure.cpp @@ -5,6 +5,12 @@ #include #include +std::string m_strVersion = "1.0.0"; +const char* wd_particleSegVersion(void) +{ + return m_strVersion.c_str(); +} + //һƽƽ //пһƽͲοƽƽ棬ߵƽеƽ //תΪƽƽ淨ΪֱIJ @@ -401,7 +407,9 @@ void wd_particleSizeMeasure( cv::imwrite("distTransformMask.png", maskImage); cv::Mat dtImage; cv::normalize(distTransform, dtImage, 0, 255, cv::NORM_MINMAX, CV_8U); - cv::imwrite("distTransform.png", dtImage); + cv::Mat dtImage_color; + cv::cvtColor(dtImage, dtImage_color, cv::COLOR_GRAY2BGR); + cv::imwrite("distTransform.png", dtImage_color); #endif //ѰPeakPeakȷPeakΪӵзˮ뷽ָ @@ -423,17 +431,45 @@ void wd_particleSizeMeasure( int filterSize = (int)filter_dt_peaks.size(); for (int i = 0; i < filterSize; i++) { + SSG_2DValueI& obj_0 = filter_dt_peaks[i]; for (int j = i + 1; j < filterSize; j++) { - + SSG_2DValueI& obj_1 = filter_dt_peaks[j]; + double dist = sqrt(pow(obj_0.x - obj_1.x, 2) + pow(obj_0.y - obj_1.y, 2)); + double distTh = dist * 1.2; + if ((obj_0.valueD + obj_1.valueD) > distTh) //ϲ + { + if (obj_0.valueD < obj_1.valueD) + obj_0.value = -1; + else + obj_1.value = -1; + } } } - + + //Ч + std::vector vld_dt_peaks; + for (int i = 0; i < filterSize; i++) + { + if (filter_dt_peaks[i].value < 0) + continue; + vld_dt_peaks.push_back(filter_dt_peaks[i]); + } +#if OUTPUT_DEBUG //debug + int dbg_seedNum = (int)vld_dt_peaks.size(); + for (int i = 0; i < dbg_seedNum; i++) + { + int dbg_px = vld_dt_peaks[i].x; + int dbg_py = vld_dt_peaks[i].y; + //dtImage_color.at(dbg_py, dbg_px) = cv::Vec3b(0, 0, 255); + cv::circle(dtImage_color, cv::Point(dbg_px, dbg_py), 3, cv::Scalar(0, 0, 255), -1); + } + cv::imwrite("distTransform_seed.png", dtImage_color); +#endif + //ˮָ - //ɨ߽磬Ŀ߽缯 //ͨĿڱ߽жǷϲĿ - //ȡСֵ double minVal, maxVal; @@ -446,13 +482,18 @@ void wd_particleSizeMeasure( wsImg.width = distTransform.cols; wsImg.height = distTransform.rows; wsImg.gray.resize(wsImg.height, std::vector(wsImg.width)); - wsImg.markers.resize(wsImg.height, std::vector(wsImg.width, 1)); // ʼͼΪ0 + wsImg.markers.resize(wsImg.height, std::vector(wsImg.width, 1)); // ʼͼΪ1, int maxValue = (int)maxVal + 2; + int maxLevel = (int)(maxVal - minVal); for (int i = 0; i < distTransform.rows; i++) { + if (i == 758) + int kkk = 1; float* rowPtr = distTransform.ptr(i); for (int j = 0; j < distTransform.cols; j++) { + if (j == 171) + int kkk = 1; float disValue = rowPtr[j]; if (disValue < 1e-4) //߽ͱ wsImg.gray[i][j] = maxValue; @@ -463,9 +504,8 @@ void wd_particleSizeMeasure( } } } - - watershed(wsImg); - + int startMarkerID = 2; + wd_seedWatershed(wsImg, vld_dt_peaks, maxLevel, startMarkerID); #if OUTPUT_DEBUG //debug cv::Mat waterShedResult(wsImg.height, wsImg.width, CV_8UC3); for (int i = 0; i < wsImg.height; ++i) { @@ -473,633 +513,104 @@ void wd_particleSizeMeasure( if (wsImg.markers[i][j] == -1) { // ˮ߽磨ɫ waterShedResult.at(i, j) = cv::Vec3b(0,0,255); } - else { // 򣨸ݱֵɲͬɫ - int color_r = (wsImg.markers[i][j] * 50) % 256; - int color_g = (color_r + 85) % 256; - int color_b = (color_r + 170) % 256; + else if (wsImg.markers[i][j] < 2) + { + waterShedResult.at(i, j) = cv::Vec3b(200, 200, 200); + } + else + { // 򣨸ݱֵɲͬɫ + + int color_r = (wsImg.markers[i][j] * 97) % 256; + int color_g = (wsImg.markers[i][j] * 73) % 256; + int color_b = (wsImg.markers[i][j] * 59) % 256; waterShedResult.at(i, j) = cv::Vec3b(color_b, color_g, color_r); } } } cv::imwrite("watershed.png", waterShedResult); #endif -#if 0 - - - - - - SSG_localPkParam searchWin; - searchWin.seachW_lines = (int)(algoParam.bagParam.bagW * 0.4); - searchWin.searchW_pts = (int)(algoParam.bagParam.bagW * 0.4); - std::vector dt_peaks; - sg_getLocalPeaks_distTransform(distTransform, dt_peaks, searchWin); - //ȡPeaks - int invlidDistToEdge = 50; //ԵPeakǷǷ㡣 - double minPeakValue = algoParam.bagParam.bagW / 8; - std::vector peaks; - for (int i = 0; i < dt_peaks.size(); i++) + //ɷָĿ + //ÿ3DͶӰϵλãĿ3D + std::vector> segObjs; + int maxMkID = startMarkerID + (int)vld_dt_peaks.size(); + segObjs.resize(maxMkID); + for (int line = 0; line < lineNum; line++) + { + for (int i = 0; i < nPointCnt; i++) { - //߽紦PeakΪϸPeakȥ - int x_diff_0 = dt_peaks[i].x; //߾룬λΪmm߶Ϊ1mm - int x_diff_1 = distTransform.cols - dt_peaks[i].x;//ұ߾ - int y_diff_0 = dt_peaks[i].y;//ϱ߾ - int y_diff_1 = distTransform.rows - dt_peaks[i].y;//±߾ - if ((x_diff_0 < invlidDistToEdge) || (x_diff_1 < invlidDistToEdge) || - (y_diff_0 < invlidDistToEdge) || (y_diff_1 < invlidDistToEdge) || - (dt_peaks[i].valueD < minPeakValue)) + SVzNL3DPosition* pt3D = &scanLines[line][i]; + pt3D->nPointIdx = 0; + if (pt3D->pt3D.z < 1e-4) continue; - //distTranformIndexingл - double pkValue = dt_peaks[i].valueD; - SSG_2DValueI a_peak = _backIndexingPeakPos(dt_peaks[i], distTranformIndexing); - a_peak.valueD = pkValue; // laser3DPoints[peaks[i].x].p3DPosition[peaks[i].y].pt3D.z; - peaks.push_back(a_peak); - } - //ո߶ - std::sort(peaks.begin(), peaks.end(), compareByHeight); - for (int i = 0, i_max = (int)peaks.size(); i < i_max; i++) - featureMask.at(peaks[i].y, peaks[i].x)[3] = 1; //peak flag + double x = pt3D->pt3D.x; + double y = pt3D->pt3D.y; + int px = (int)(x - x_range.min) / scale + edgeSkip; + int py = (int)(y - y_range.min) / scale + edgeSkip; -#if 0 - //ʹʵzֵ任ֵ - for (int i = 0, i_max = peaks.size(); i < i_max; i++) - { - peaks[i].valueD = laser3DPoints[peaks[i].x].p3DPosition[peaks[i].y].pt3D.z; - } -#endif - - /// ߵΪӽ - std::vector peakRgns; - int peakRgnId = 1; - for (int i = 0, i_max = (int)peaks.size(); i < i_max; i++) - { - if (i == 3) - int kkk = 1; - - SVzNL3DPosition* pk_pt = &(laser3DPoints[peaks[i].x].p3DPosition[peaks[i].y]); - int pkRgnId = (pk_pt->nPointIdx >> 8) & 0xff; - if (pkRgnId > 0) - continue; - - // - //ˮƽʹֱɨõ߽ - std::vector< SSG_lineConotours> topContour; - std::vector< SSG_lineConotours> bottomContour; - std::vector< SSG_lineConotours> leftContour; - std::vector< SSG_lineConotours> rightContour; - int maxEdgeId_top = 0, maxEdgeId_btm = 0, maxEdgeId_left = 0, maxEdgeId_right = 0; - sg_peakXYScan( - laser3DPoints, - lineNum, - featureMask, - peaks[i], - algoParam.growParam, - algoParam.bagParam, - false, - topContour, - bottomContour, - leftContour, - rightContour, - &maxEdgeId_top, - &maxEdgeId_btm, - &maxEdgeId_left, - &maxEdgeId_right); - - - int vldSide = 0; - if (leftContour.size() > 30) - vldSide++; - if (rightContour.size() > 30) - vldSide++; - if (topContour.size() > 30) - vldSide++; - if (bottomContour.size() > 30) - vldSide++; - - int invldSide = 0; - if (leftContour.size() < 10) - invldSide++; - if (rightContour.size() < 10) - invldSide++; - if (topContour.size() < 10) - invldSide++; - if (bottomContour.size() < 10) - invldSide++; - - if ((vldSide < 3) || (invldSide > 0)) - continue; - - //ȫƥ䣺϶ĴӣҪȫƥѰҵȷı - std::vector matchTable_TB; - std::vector< SSG_matchPair> TB_pairs; - std::vector TB_contourPairs; - int TB_matchNum = 0; - _getMatchTable( - topContour, - bottomContour, - maxEdgeId_top, - maxEdgeId_btm, - true, //isVScan, - vTreeStart, - hTreeStart, - matchTable_TB, - TB_pairs, - TB_contourPairs, - &TB_matchNum); - if (TB_matchNum < 25) - continue; - - std::vector< SSG_matchPair> LR_pairs; - std::vector matchTable_LR; - std::vector LR_contourPairs; - int LR_matchNum = 0; - _getMatchTable( - leftContour, - rightContour, - maxEdgeId_left, - maxEdgeId_right, - false, //isHScan, - vTreeStart, - hTreeStart, - matchTable_LR, - LR_pairs, - LR_contourPairs, - &LR_matchNum); - - if (LR_matchNum < 25) - continue; - - int lowLevelChkFlag = 0; - SSG_peakRgnInfo a_pkRgn = _maxLikelihoodMatch( - laser3DPoints, - lineNum, - hvTreeSize, - peaks[i], - matchTable_TB, - TB_pairs, - TB_contourPairs, - TB_matchNum, - maxEdgeId_btm, - matchTable_LR, - LR_pairs, - LR_contourPairs, - LR_matchNum, - maxEdgeId_right, - allTreesInfo, - vTreeStart, - hTreeStart, - globalROI, - algoParam, - peakRgnId, - &lowLevelChkFlag); - if (a_pkRgn.pkRgnIdx > 0) + int marker = wsImg.markers[py][px]; + if ((marker >= startMarkerID)&&( marker <= maxMkID)) { - peakRgns.push_back(a_pkRgn); - peakRgnId++; + pt3D->nPointIdx = marker; + segObjs[marker].push_back(pt3D->pt3D); } } -#if 1 - ///֤ûдڴӵĿδ - ///ʣµĿ꣬ûмʹС - ///ɨʱⱣ֤ˮƽֱĿǷӳߴ磬Ȼһ - while (1) + } + + //Ŀ + for (int i = startMarkerID; i < maxMkID; i++) + { + if (segObjs[i].size() < 10) + continue; + + //ȡͶӰ,ͳСZZ + double minZ = -1; + double maxZ = 0; + std::vector points; + int ptSize = (int)segObjs[i].size(); + for (int m = 0; m < ptSize; m++) { - std::vector iter_objs; - //ûдPeak㱣 - std::vector residualPeaks; - for (int i = 0, i_max = (int)peaks.size(); i < i_max; i++) + float x = (float)segObjs[i][m].x; + float y = (float)segObjs[i][m].y; + points.push_back(cv::Point2f(x, y)); + if (minZ < 0) { - SVzNL3DPosition* pk_pt = &(laser3DPoints[peaks[i].x].p3DPosition[peaks[i].y]); - int pkRgnId = (pk_pt->nPointIdx >> 8) & 0xff; - if (pkRgnId == 0) - { - residualPeaks.push_back(peaks[i]); - } - } - if (residualPeaks.size() == 0) - break; - - bool rgnPtAsEdge = true; - for (int ri = 0; ri < residualPeaks.size(); ri++) - { - SVzNL3DPosition* pk_pt = &(laser3DPoints[residualPeaks[ri].x].p3DPosition[residualPeaks[ri].y]); - int pkRgnId = (pk_pt->nPointIdx >> 8) & 0xff; - if (pkRgnId > 0) - continue; - // - //ˮƽʹֱɨõ߽ - std::vector< SSG_lineConotours> resi_topContour; - std::vector< SSG_lineConotours> resi_bottomContour; - std::vector< SSG_lineConotours> resi_leftContour; - std::vector< SSG_lineConotours> resi_rightContour; - int resi_maxEdgeId_top = 0, resi_maxEdgeId_btm = 0, resi_maxEdgeId_left = 0, resi_maxEdgeId_right = 0; - sg_peakXYScan( - laser3DPoints, - lineNum, - featureMask, - residualPeaks[ri], - algoParam.growParam, - algoParam.bagParam, - true, - resi_topContour, - resi_bottomContour, - resi_leftContour, - resi_rightContour, - &resi_maxEdgeId_top, - &resi_maxEdgeId_btm, - &resi_maxEdgeId_left, - &resi_maxEdgeId_right); - - if ((resi_topContour.size() == 0) || (resi_bottomContour.size() == 0) || (resi_leftContour.size() == 0) || (resi_rightContour.size() == 0)) - continue; - - //ֶμƽȺƽ߶ԼֶROI - //ȫƥ䣨ˮƽзֶεпܾ - std::vector matchTable_TB; - std::vector< SSG_matchPair> TB_pairs; - std::vector TB_contourPairs; - int TB_matchNum = 0; - _getMatchTable( - resi_topContour, - resi_bottomContour, - resi_maxEdgeId_top, - resi_maxEdgeId_btm, - true, //isVScan, - vTreeStart, - hTreeStart, - matchTable_TB, - TB_pairs, - TB_contourPairs, - &TB_matchNum); - - if (TB_matchNum < 25) - continue; - - std::vector< SSG_matchPair> LR_pairs; - std::vector matchTable_LR; - std::vector LR_contourPairs; - int LR_matchNum = 0; - _getMatchTable( - resi_leftContour, - resi_rightContour, - resi_maxEdgeId_left, - resi_maxEdgeId_right, - false, //isHScan, - vTreeStart, - hTreeStart, - matchTable_LR, - LR_pairs, - LR_contourPairs, - &LR_matchNum); - - if (LR_matchNum < 25) - continue; - - int lowLevelChkFlag = 0; - SSG_peakRgnInfo a_pkRgn = _maxLikelihoodMatch( - laser3DPoints, - lineNum, - hvTreeSize, - peaks[ri], - matchTable_TB, - TB_pairs, - TB_contourPairs, - TB_matchNum, - resi_maxEdgeId_btm, - matchTable_LR, - LR_pairs, - LR_contourPairs, - LR_matchNum, - resi_maxEdgeId_right, - allTreesInfo, - vTreeStart, - hTreeStart, - globalROI, - algoParam, - peakRgnId, - &lowLevelChkFlag); -#if 0 - if (lowLevelChkFlag > 0) - { - //lowLevelFlag_T + lowLevelFlag_B<<1 + lowLevelFlag_L<<2 + lowLevelFlag_R<<3; - if (lowLevelChkFlag & 0x01) //Top - { - - } - if (lowLevelChkFlag & 0x02) //Bottom - { - } - if (lowLevelChkFlag & 0x04) //Left - { - } - if (lowLevelChkFlag & 0x08) //Rigjt - { - } - } -#endif - if (a_pkRgn.pkRgnIdx > 0) - { - iter_objs.push_back(a_pkRgn); - peakRgnId++; - } - } - if (iter_objs.size() == 0) - break; - - peakRgns.insert(peakRgns.end(), iter_objs.begin(), iter_objs.end()); - //Ϊһε׼ - peaks.clear(); - peaks.insert(peaks.end(), residualPeaks.begin(), residualPeaks.end()); - } - - ///ʣСĿײ - ///δ֪δԷֹδ֪ײ - ///¼еĴ1/4L*1/4WĿ - std::vector smallObjPeaks; //¼0.25L * 0.25WĿ꣬ײ - //ûдPeak㱣 - std::vector residualPeaks; - for (int i = 0, i_max = (int)peaks.size(); i < i_max; i++) - { - SVzNL3DPosition* pk_pt = &(laser3DPoints[peaks[i].x].p3DPosition[peaks[i].y]); - int pkRgnId = (pk_pt->nPointIdx >> 8) & 0xff; - if (pkRgnId == 0) - { - residualPeaks.push_back(peaks[i]); - } - } - if (residualPeaks.size() > 0) - { - bool rgnPtAsEdge = true; - for (int ri = 0; ri < residualPeaks.size(); ri++) - { - // - //ˮƽʹֱɨõ߽ - std::vector< SSG_lineConotours> resi_topContour; - std::vector< SSG_lineConotours> resi_bottomContour; - std::vector< SSG_lineConotours> resi_leftContour; - std::vector< SSG_lineConotours> resi_rightContour; - int resi_maxEdgeId_top = 0, resi_maxEdgeId_btm = 0, resi_maxEdgeId_left = 0, resi_maxEdgeId_right = 0; - sg_peakXYScan( - laser3DPoints, - lineNum, - featureMask, - residualPeaks[ri], - algoParam.growParam, - algoParam.bagParam, - true, - resi_topContour, - resi_bottomContour, - resi_leftContour, - resi_rightContour, - &resi_maxEdgeId_top, - &resi_maxEdgeId_btm, - &resi_maxEdgeId_left, - &resi_maxEdgeId_right); - //ROI - //޵ - SSG_ROIRectD objROI = { 0, -1, 0, 0 }; - for (int n = 0; n < resi_topContour.size(); n++) - { - std::vector& a_line_contourPts = resi_topContour[n].contourPts; - for (int m = 0; m < a_line_contourPts.size(); m++) - { - if (objROI.right < objROI.left) - { - objROI.left = a_line_contourPts[m].edgePt.x; - objROI.right = a_line_contourPts[m].edgePt.x; - objROI.top = a_line_contourPts[m].edgePt.y; - objROI.bottom = a_line_contourPts[m].edgePt.y; - } - else - { - objROI.left = objROI.left > a_line_contourPts[m].edgePt.x ? a_line_contourPts[m].edgePt.x : objROI.left; - objROI.right = objROI.right < a_line_contourPts[m].edgePt.x ? a_line_contourPts[m].edgePt.x : objROI.right; - objROI.top = objROI.top > a_line_contourPts[m].edgePt.y ? a_line_contourPts[m].edgePt.y : objROI.top; - objROI.bottom = objROI.bottom < a_line_contourPts[m].edgePt.y ? a_line_contourPts[m].edgePt.y : objROI.bottom; - } - } - } - for (int n = 0; n < resi_bottomContour.size(); n++) - { - std::vector& a_line_contourPts = resi_bottomContour[n].contourPts; - for (int m = 0; m < a_line_contourPts.size(); m++) - { - if (objROI.right < objROI.left) - { - objROI.left = a_line_contourPts[m].edgePt.x; - objROI.right = a_line_contourPts[m].edgePt.x; - objROI.top = a_line_contourPts[m].edgePt.y; - objROI.bottom = a_line_contourPts[m].edgePt.y; - } - else - { - objROI.left = objROI.left > a_line_contourPts[m].edgePt.x ? a_line_contourPts[m].edgePt.x : objROI.left; - objROI.right = objROI.right < a_line_contourPts[m].edgePt.x ? a_line_contourPts[m].edgePt.x : objROI.right; - objROI.top = objROI.top > a_line_contourPts[m].edgePt.y ? a_line_contourPts[m].edgePt.y : objROI.top; - objROI.bottom = objROI.bottom < a_line_contourPts[m].edgePt.y ? a_line_contourPts[m].edgePt.y : objROI.bottom; - } - } - } - for (int n = 0; n < resi_leftContour.size(); n++) - { - std::vector& a_line_contourPts = resi_leftContour[n].contourPts; - for (int m = 0; m < a_line_contourPts.size(); m++) - { - if (objROI.right < objROI.left) - { - objROI.left = a_line_contourPts[m].edgePt.x; - objROI.right = a_line_contourPts[m].edgePt.x; - objROI.top = a_line_contourPts[m].edgePt.y; - objROI.bottom = a_line_contourPts[m].edgePt.y; - } - else - { - objROI.left = objROI.left > a_line_contourPts[m].edgePt.x ? a_line_contourPts[m].edgePt.x : objROI.left; - objROI.right = objROI.right < a_line_contourPts[m].edgePt.x ? a_line_contourPts[m].edgePt.x : objROI.right; - objROI.top = objROI.top > a_line_contourPts[m].edgePt.y ? a_line_contourPts[m].edgePt.y : objROI.top; - objROI.bottom = objROI.bottom < a_line_contourPts[m].edgePt.y ? a_line_contourPts[m].edgePt.y : objROI.bottom; - } - } - } - for (int n = 0; n < resi_rightContour.size(); n++) - { - std::vector& a_line_contourPts = resi_rightContour[n].contourPts; - for (int m = 0; m < a_line_contourPts.size(); m++) - { - if (objROI.right < objROI.left) - { - objROI.left = a_line_contourPts[m].edgePt.x; - objROI.right = a_line_contourPts[m].edgePt.x; - objROI.top = a_line_contourPts[m].edgePt.y; - objROI.bottom = a_line_contourPts[m].edgePt.y; - } - else - { - objROI.left = objROI.left > a_line_contourPts[m].edgePt.x ? a_line_contourPts[m].edgePt.x : objROI.left; - objROI.right = objROI.right < a_line_contourPts[m].edgePt.x ? a_line_contourPts[m].edgePt.x : objROI.right; - objROI.top = objROI.top > a_line_contourPts[m].edgePt.y ? a_line_contourPts[m].edgePt.y : objROI.top; - objROI.bottom = objROI.bottom < a_line_contourPts[m].edgePt.y ? a_line_contourPts[m].edgePt.y : objROI.bottom; - } - } - } - //ROIС - double obj_L = objROI.right - objROI.left; - double obj_W = objROI.bottom - objROI.top; - if (obj_L < obj_W) - { - double tmp_value = obj_W; - obj_W = obj_L; - obj_L = tmp_value; - } - if ((obj_L > algoParam.bagParam.bagL * 0.25) && (obj_W > algoParam.bagParam.bagW * 0.25)) - { - smallObjPeaks.push_back(residualPeaks[ri]); - } - } - } - -#endif - - //Ŀ򣺷ֲ -> ߲ -> ߲һд - if (peakRgns.size() > 0) - { - double maxHeight = peakRgns[0].centerPos.z; - for (int i = 1; i < peakRgns.size(); i++) - { - if (maxHeight > peakRgns[i].centerPos.z) - maxHeight = peakRgns[i].centerPos.z; - } - //ȡͬ߶ȲĿ - std::vector level0_objs; - for (int i = 0, i_max = (int)peakRgns.size(); i < i_max; i++) - { - double z_diff = peakRgns[i].centerPos.z - maxHeight; - if (z_diff < algoParam.bagParam.bagH / 2) //ֲ - { - level0_objs.push_back(peakRgns[i]); - } - } - peakRgns.clear(); - peakRgns.insert(peakRgns.end(), level0_objs.begin(), level0_objs.end()); - int level0_size = (int)peakRgns.size(); - if (level0_size > 1) //һ򣬷 - { - //ȡYСĿ - double minY = 0; - double minY_idx = -1; - for (int i = 0; i < level0_size; i++) - { - if (minY_idx < 0) - { - minY = peakRgns[i].centerPos.y; - minY_idx = i; - } - else - { - if (minY > peakRgns[i].centerPos.y) - { - minY = peakRgns[i].centerPos.y; - minY_idx = i; - } - } - } - std::vector row_0_outlier; - for (int i = 0; i < level0_size; i++) - { - double y_diff = peakRgns[i].centerPos.y - minY; - if (y_diff < algoParam.bagParam.bagW / 2) //һ - objOps.push_back(peakRgns[i]); - else - row_0_outlier.push_back(i); //еż¼ - } - //ԵһеĿ갴 - if (objOps.size() > 1) - { - std::sort(objOps.begin(), objOps.end(), compareByXValue); - } - for (int i = 0; i < row_0_outlier.size(); i++) - objOps.push_back(peakRgns[row_0_outlier[i]]); -#if 0 - for (int i = level0_end + 1; i < peakRgns.size(); i++) - objOps.push_back(peakRgns[i]); -#endif + minZ = segObjs[i][m].z; + maxZ = segObjs[i][m].z; } else - objOps.insert(objOps.end(), peakRgns.begin(), peakRgns.end()); - } - //ײ - if ((objOps.size() > 0) && (smallObjPeaks.size() > 0)) - { - SSG_peakRgnInfo* highest_obj = &objOps[0]; - double objZ = highest_obj->centerPos.z; - for (int i = 0; i < smallObjPeaks.size(); i++) { - SSG_2DValueI* a_samllPk = &smallObjPeaks[i]; - if (highest_obj->centerPos.z > a_samllPk->valueD + algoParam.bagParam.bagH / 2) - { - SVzNL3DPosition* smallPkPt = &laser3DPoints[a_samllPk->x].p3DPosition[a_samllPk->y]; - double dist = sqrt(pow(highest_obj->centerPos.x - smallPkPt->pt3D.x, 2) + pow(highest_obj->centerPos.y - smallPkPt->pt3D.y, 2)); - double dia_angle = sqrt(pow(highest_obj->objSize.dWidth, 2) + pow(highest_obj->objSize.dHeight, 2)); - //ͬȽ - double z_diff = smallPkPt->pt3D.z - objZ; - if (z_diff < algoParam.bagParam.bagH / 2) //ֲ - { - if (dist < dia_angle / 2) - objOps.clear(); //μЧ - } - } + if (minZ > segObjs[i][m].z) minZ = segObjs[i][m].z; + if (maxZ < segObjs[i][m].z) maxZ = segObjs[i][m].z; } } - //ͶԭϵԱ۱궨ȷ - for (int i = 0; i < lineNum; i++) - sg_lineDataR(&laser3DPoints[i], poseCalibPara.invRMatrix, -1); - //Ͷԭϵ - double invMatrix[3][3]; - invMatrix[0][0] = poseCalibPara.invRMatrix[0]; - invMatrix[0][1] = poseCalibPara.invRMatrix[1]; - invMatrix[0][2] = poseCalibPara.invRMatrix[2]; - invMatrix[1][0] = poseCalibPara.invRMatrix[3]; - invMatrix[1][1] = poseCalibPara.invRMatrix[4]; - invMatrix[1][2] = poseCalibPara.invRMatrix[5]; - invMatrix[2][0] = poseCalibPara.invRMatrix[6]; - invMatrix[2][1] = poseCalibPara.invRMatrix[7]; - invMatrix[2][2] = poseCalibPara.invRMatrix[8]; - for (int i = 0, i_max = (int)objOps.size(); i < i_max; i++) + if (points.size() == 0) + continue; + //СӾ + // СӾ + cv::RotatedRect rect = minAreaRect(points); + cv::Point2f vertices[4]; + rect.points(vertices); + double width = rect.size.width; //ͶӰĿ͸ߣ ӦӵijͿ + double height = rect.size.height; + if (width < height) { - SSG_EulerAngles euAngle = { objOps[i].centerPos.x_roll, objOps[i].centerPos.y_pitch, objOps[i].centerPos.z_yaw }; - double pose[3][3]; - eulerToRotationMatrixZYX(euAngle, pose); - double resultMatrix[3][3]; - for (int i = 0; i < 3; i++) - { - for (int j = 0; j < 3; j++) - { - resultMatrix[i][j] = 0; - for (int m = 0; m < 3; m++) - resultMatrix[i][j] += invMatrix[i][m] * pose[m][j]; - } - } - SSG_EulerAngles resultEuAngle = rotationMatrixToEulerZYX(resultMatrix); - objOps[i].centerPos.z_yaw = resultEuAngle.yaw; - double x = objOps[i].centerPos.x * poseCalibPara.invRMatrix[0] + - objOps[i].centerPos.y * poseCalibPara.invRMatrix[1] + - objOps[i].centerPos.z * poseCalibPara.invRMatrix[2]; - double y = objOps[i].centerPos.x * poseCalibPara.invRMatrix[3] + - objOps[i].centerPos.y * poseCalibPara.invRMatrix[4] + - objOps[i].centerPos.z * poseCalibPara.invRMatrix[5]; - double z = objOps[i].centerPos.x * poseCalibPara.invRMatrix[6] + - objOps[i].centerPos.y * poseCalibPara.invRMatrix[7] + - objOps[i].centerPos.z * poseCalibPara.invRMatrix[8]; - objOps[i].centerPos.x = x; - objOps[i].centerPos.y = y; - objOps[i].centerPos.z = z; + double tmp = height; + height = width; + width = tmp; } -#endif - //ˮƽʹֱȡ - - //о任 - //ˮָ - //z߶ȼĿ + SWD_ParticlePosInfo a_obj; + a_obj.size.length = width; + a_obj.size.width = height; + a_obj.size.height = maxZ - minZ; + for (int m = 0; m < 4; m++) + { + SVzNL3DPoint vPt_btm = { vertices[m].x, vertices[m].y, maxZ }; + SVzNL3DPoint vPt_top = { vertices[m].x, vertices[m].y, minZ }; + a_obj.vertix[m] = vPt_btm; + a_obj.vertix[m + 4] = vPt_top; + } + particles.push_back(a_obj); + } } \ No newline at end of file diff --git a/sourceCode/WD_particleSizeMeasure_Export.h b/sourceCode/WD_particleSizeMeasure_Export.h index 133ddda..62d26cb 100644 --- a/sourceCode/WD_particleSizeMeasure_Export.h +++ b/sourceCode/WD_particleSizeMeasure_Export.h @@ -1,16 +1,11 @@ #pragma once -#if defined(SG_API_LIBRARY) -# define SG_APISHARED_EXPORT __declspec(dllexport) -#else -# define SG_APISHARED_EXPORT __declspec(dllimport) -#endif - +#include "SG_baseAlgo_Export.h" #include "SG_baseDataType.h" #include #include -#define OUTPUT_DEBUG 1 +#define OUTPUT_DEBUG 0 typedef struct { @@ -27,10 +22,14 @@ typedef struct typedef struct { - double EQRadius; - SVzNL3DPoint center_pos; + //double EQRadius; + SWD_sizeParam size; + SVzNL3DPoint vertix[8]; }SWD_ParticlePosInfo; +//汾 +SG_APISHARED_EXPORT const char* wd_particleSegVersion(void); + //һƽƽ //пһƽͲοƽƽ棬ߵƽеƽ //תΪƽƽ淨ΪֱIJ diff --git a/sourceCode/WD_watershed.cpp b/sourceCode/WD_watershed.cpp index a718f89..5da6894 100644 --- a/sourceCode/WD_watershed.cpp +++ b/sourceCode/WD_watershed.cpp @@ -246,6 +246,9 @@ void saveResult(const string& filename, const Image& img) { const int dx[] = { -1, -1, -1, 0, 0, 1, 1, 1 }; const int dy[] = { -1, 0, 1, -1, 1, -1, 0, 1 }; +const int dx4[] = { -1, 0, 0, 1}; +const int dy4[] = { 0, -1, 1, 0}; + // ֲСֵΪʼӵ void findMinima(SWD_waterShedImage& img, int& markerCount) { markerCount = 1; //ӦID @@ -302,7 +305,7 @@ void findMinima(SWD_waterShedImage& img, int& markerCount) { } // ˮ㷨 -void watershed(SWD_waterShedImage& img) +void watershed(SWD_waterShedImage& img) { int markerCount; findMinima(img, markerCount); // Զӵ @@ -311,7 +314,7 @@ void watershed(SWD_waterShedImage& img) vector>> pixels; // (Ҷֵ, (x,y)) for (int i = 0; i < img.height; ++i) { for (int j = 0; j < img.width; ++j) { - pixels.emplace_back(img.gray[i][j], make_pair(i, j)); + pixels.emplace_back(img.gray[i][j], make_pair(j, i)); } } sort(pixels.begin(), pixels.end()); @@ -348,19 +351,156 @@ void watershed(SWD_waterShedImage& img) } } -#if 0 -int main() { - Image img; - if (!readPPM("input.ppm", img)) { // PPMͼ - cerr << "޷ȡͼ" << endl; - return -1; +//ģעˮעˮڿʼͬˮλע +// inlet: עˮ +// level: ˮλ +void waterInjection(SWD_waterShedImage& img, SVzNL2DPoint inlet, int level) +{ + int py = inlet.y; + int px = inlet.x; + int marker = img.markers[py][px]; + std::vector levelPts; + levelPts.push_back(inlet); + int readPtr = 0; + while (readPtr < levelPts.size()) + { + SVzNL2DPoint seed = levelPts[readPtr]; + readPtr++; + + //4ڽ + for (int d = 0; d < 4; ++d) + { + int nx = seed.x + dx4[d]; + int ny = seed.y + dy4[d]; + if (ny >= 0 && ny < img.height && nx >= 0 && nx < img.width) + { + if ((img.gray[ny][nx] == level) && (img.markers[ny][nx] == 0)) + { + img.markers[ny][nx] = marker; + SVzNL2DPoint nxt_seed = { nx, ny }; + levelPts.push_back(nxt_seed); + } + } + } + } +} + +bool _checkMarkerExist(std::vector& MarkerLst, int marker) +{ + bool exist = false; + for (int i = 0, i_max = (int)MarkerLst.size(); i < i_max; i++) + { + if (MarkerLst[i] == marker) + { + exist = true; + break; + } + } + return exist; +} + +// ӵзˮ㷨 +// watershedSeeds:ӵ +// maxLevel:ˮλֵ +void wd_seedWatershed(SWD_waterShedImage& img, std::vector& watershedSeeds, int maxLevel, int startMakerID) +{ + int markerCount = startMakerID; + int seedSize = (int)watershedSeeds.size(); + for (int i = 0; i < seedSize; i++) + { + int px = watershedSeeds[i].x; + int py = watershedSeeds[i].y; + watershedSeeds[i].value = markerCount; + img.markers[py][px] = markerCount; // ӵ + int greyValue = img.gray[py][px]; //ˮλ + SVzNL2DPoint inlet = { px, py }; //עˮ + waterInjection(img, inlet, greyValue);//עˮͬһˮλӵIJע + markerCount++; } - watershed(img); // ִзˮָ - saveResult("watershed_result.ppm", img); // + // Ҷֵأģˮλ + std::vector> levelPtList; + levelPtList.resize(maxLevel + 1); + for (int y = 0; y < img.height; ++y) + { + for (int x = 0; x < img.width; ++x) + { + int level = img.gray[y][x]; + if (level <= maxLevel) + { + SVzNL2DPoint a_pt = { x, y }; + levelPtList[level].push_back(a_pt); + } + } + } - cout << "ָɣѱΪ watershed_result.ppm" << endl; - return 0; + while (1) + { + bool allDone = true; + // أִעˮ + for (int i = 0; i <= maxLevel; i++) + { + if (levelPtList[i].size() == 0) + continue; + + //ͬһˮλlevel)ִеעˮ + bool doInjection = true; + while (doInjection) + { + int injectionNum = 0; + std::vector resiPts; + int ptSize = (int)levelPtList[i].size(); + for (int pi = 0; pi < ptSize; pi++) + { + int x = levelPtList[i][pi].x; + int y = levelPtList[i][pi].y; + if (img.markers[y][x] > 0) continue; // ѱǵӵ + + // ռѱǵ + vector neighborMarkers; + for (int d = 0; d < 8; ++d) + { + int nx = x + dx[d]; + int ny = y + dy[d]; + if (ny >= 0 && ny < img.height && nx >= 0 && nx < img.width) + { + int m = img.markers[ny][nx]; + if (m > 0) + { + bool exist = _checkMarkerExist(neighborMarkers, m); + if (false == exist) + neighborMarkers.push_back(m); + } + } + } + + if (neighborMarkers.empty()) + { + resiPts.push_back(levelPtList[i][pi]);// ǣȴ + } + else if (neighborMarkers.size() == 1) { + img.markers[y][x] = neighborMarkers[0]; // ͬһ + injectionNum++; + } + else { + // 򽻻㣬Ϊˮ + img.markers[y][x] = -1; + injectionNum++; + } + } + levelPtList[i].clear(); + levelPtList[i].insert(levelPtList[i].end(), resiPts.begin(), resiPts.end()); + + if ((injectionNum == 0) || (levelPtList[i].size() == 0)) + { + doInjection = false; + if (levelPtList[i].size() > 0) + allDone = false; + } + } + } + if (true == allDone) + break; + } } -#endif #endif \ No newline at end of file