From 9aca73cfa4fc388bef1844552a3f390051b0f606 Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Wed, 22 Jul 2026 11:23:21 +0800 Subject: [PATCH] =?UTF-8?q?hybridPosePositioning=20version=201.1.0=20:=20?= =?UTF-8?q?=E6=B5=B7=E7=91=9E=E9=A9=AC=E7=A0=81=E5=9E=9B=E8=A7=84=E5=88=92?= =?UTF-8?q?=E7=89=88=E6=9C=AC?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../hybridPosePositioning.vcxproj | 160 ++ .../hybridPosePositioning_test.cpp | 2332 +++++++++++++++++ .../hybridPosePositioning_test.vcxproj | 145 + sourceCode/hybridPosePositioning.cpp | 1289 +++++++++ sourceCode/hybridPosePositioning_Export.h | 101 + 5 files changed, 4027 insertions(+) create mode 100644 hybridPosePositioning/hybridPosePositioning.vcxproj create mode 100644 hybridPosePositioning_test/hybridPosePositioning_test.cpp create mode 100644 hybridPosePositioning_test/hybridPosePositioning_test.vcxproj create mode 100644 sourceCode/hybridPosePositioning.cpp create mode 100644 sourceCode/hybridPosePositioning_Export.h diff --git a/hybridPosePositioning/hybridPosePositioning.vcxproj b/hybridPosePositioning/hybridPosePositioning.vcxproj new file mode 100644 index 0000000..882559f --- /dev/null +++ b/hybridPosePositioning/hybridPosePositioning.vcxproj @@ -0,0 +1,160 @@ + + + + + Debug + Win32 + + + Release + Win32 + + + Debug + x64 + + + Release + x64 + + + + + + + + + + 17.0 + Win32Proj + {4b6eda25-fced-4ae3-ba12-aa559d8b71dc} + hybridPosePositioning + 10.0 + + + + DynamicLibrary + true + v143 + Unicode + + + DynamicLibrary + false + v143 + true + Unicode + + + DynamicLibrary + true + v143 + Unicode + + + DynamicLibrary + false + v143 + true + Unicode + + + + + + + + + + + + + + + + + + + + + $(SolutionDir)build\$(Platform)\$(Configuration)\ + ..\..\thirdParty\VzNLSDK\Inc;..\sourceCode;..\sourceCode\inc;$(IncludePath) + + + $(SolutionDir)build\$(Platform)\$(Configuration)\ + ..\..\thirdParty\VzNLSDK\Inc;..\sourceCode;..\sourceCode\inc;$(IncludePath) + + + + Level3 + true + WIN32;_DEBUG;HYBRIDPOSEPOSITIONING_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) + true + Use + pch.h + + + Windows + true + false + + + + + Level3 + true + true + true + WIN32;NDEBUG;HYBRIDPOSEPOSITIONING_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) + true + Use + pch.h + + + Windows + true + false + + + + + Level3 + true + _CRT_SECURE_NO_WARNINGS;_DEBUG;HYBRIDPOSEPOSITIONING_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) + true + NotUsing + pch.h + ..\..\thirdParty\opencv320\build\include;%(AdditionalIncludeDirectories) + + + Windows + true + false + ..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Debug;%(AdditionalLibraryDirectories) + opencv_world320d.lib;baseAlgorithm.lib;%(AdditionalDependencies) + + + + + Level3 + true + true + true + _CRT_SECURE_NO_WARNINGS;NDEBUG;HYBRIDPOSEPOSITIONING_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) + true + NotUsing + pch.h + ..\..\thirdParty\opencv320\build\include;%(AdditionalIncludeDirectories) + + + Windows + 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/hybridPosePositioning_test/hybridPosePositioning_test.cpp b/hybridPosePositioning_test/hybridPosePositioning_test.cpp new file mode 100644 index 0000000..f635656 --- /dev/null +++ b/hybridPosePositioning_test/hybridPosePositioning_test.cpp @@ -0,0 +1,2332 @@ +#include +#include +#include +#include +#include +#include "direct.h" +#include +#include "hybridPosePositioning_Export.h" +#include +#include +#include +#include "SG_baseAlgo_Export.h" + +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; + +SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, double matrix3d[9]) +{ + SVzNL3DPoint _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; +} + +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 _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(); +} + +//读取3D和2D数据 +void vzReadLaserScanPointFromFile_XYZUV_vector(const char* fileName, std::vector>& scanData) +{ + std::ifstream inputFile(fileName); + std::string linedata; + + if (inputFile.is_open() == false) + return; + + std::vector< SVzNLPositionD> 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; + float leftU, leftV; + float rightU, rightV; + sscanf_s(linedata.c_str(), "{ %f, %f, %f }-{ %f, %f }-{ %f, %f }", &X, &Y, &Z, &leftU, &leftV, &rightU, &rightV); + if (Z > 1e-4) + int kkk = 1; + SVzNLPositionD a_pt; + a_pt.ptLeft2D.x = leftU; + a_pt.ptLeft2D.y = leftV; + a_pt.ptRight2D.x = rightU; + a_pt.ptRight2D.y = rightV; + 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 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; +} + +int counterLinePtNum_XYZ(std::vector< SVzNL3DPosition>& lineData) +{ + int num = 0; + for (int i = 0; i < (int)lineData.size(); i++) + { + if (lineData[i].pt3D.z > 1e-4) + num++; + } + return num; +} + +int counterLinePtNum_XYZUV(std::vector< SVzNLPositionD>& lineData) +{ + int num = 0; + for (int i = 0; i < (int)lineData.size(); i++) + { + if (lineData[i].pt3D.z > 1e-4) + num++; + } + return num; +} + +void _removeZeroLines_XYZ(std::vector>& scanData) +{ + int lineNum = (int)scanData.size(); + int firstLine = -1; + int lastLine = 0; + for (int line = 0; line < lineNum; line++) + { + int num = counterLinePtNum_XYZ(scanData[line]); + if ((num > 0) && (firstLine < 0)) + firstLine = line; + if (num > 0) + lastLine = line; + } + if (firstLine < 0) + { + scanData.clear(); + return; + } + + if ((lastLine < (lineNum - 1)) && (lastLine > 0)) + scanData.erase(scanData.begin() + lastLine + 1, scanData.end()); + + scanData.erase(scanData.begin(), scanData.begin() + firstLine); + return; +} + +void _removeZeroLines_XYZUV(std::vector>& scanData) +{ + int lineNum = (int)scanData.size(); + int firstLine = -1; + int lastLine = 0; + for (int line = 0; line < lineNum; line++) + { + int num = counterLinePtNum_XYZUV(scanData[line]); + if ((num > 0) && (firstLine < 0)) + firstLine = line; + if (num > 0) + lastLine = line; + } + if (firstLine < 0) + { + scanData.clear(); + return; + } + + if ((lastLine < (lineNum - 1)) && (lastLine > 0)) + scanData.erase(scanData.begin() + lastLine + 1, scanData.end()); + + scanData.erase(scanData.begin(), scanData.begin() + firstLine); + return; +} + +void vzReadObj2DROI(const char* fileName, std::vector& objROIs) +{ + std::ifstream inputFile(fileName); + std::string linedata; + + if (inputFile.is_open() == false) + return; + + while (getline(inputFile, linedata)) + { + if (0 == strncmp("obj_", linedata.c_str(), 4)) + { + int objID; + char type; + float L, T, W, H, score; + sscanf_s(linedata.c_str(), "obj_%d: C=%c, Rect={ %f, %f, %f, %f }, Score=%f", + &objID, &type, sizeof(type), &L, &T, &W, &H, &score); + WD_objArea2D a_roi; + if ('a' == type) + a_roi.workpieceType = 1; + else if ('b' == type) + a_roi.workpieceType = 2; + else if ('c' == type) + a_roi.workpieceType = 3; + a_roi.roi.left = L; + a_roi.roi.top = T; + a_roi.roi.right = a_roi.roi.left + W -1; + a_roi.roi.bottom = a_roi.roi.top + H - 1; + a_roi.score2D = score; + objROIs.push_back(a_roi); + } + } + inputFile.close(); + return; +} + +void _outputRGBDResult_XYZUV_RGBD( + char* fileName, + std::vector>& scanLines, + std::vector< WD_workpieceInfo>& workpiecePositions) +{ + std::vector objects; + int objNumber = (int)workpiecePositions.size(); + for (int i = 0; i < objNumber; i++) + { + SVzNL3DPosition a_objPt; + a_objPt.pt3D = workpiecePositions[i].center; + objects.push_back(a_objPt); + } + + int lineNum = (int)scanLines.size(); + std::ofstream sw(fileName); + int realLines = (objNumber == 0) ? lineNum : (lineNum + 1); + 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 = (int)scanLines[line].size(); + if (linePtNum == 0) + continue; + + sw << "Line_" << lineIdx << "_0_" << linePtNum << std::endl; + lineIdx++; + for (int i = 0; i < linePtNum; i++) + { + SVzNLPositionD* pt3D = &scanLines[line][i]; + if (pt3D->nPointIdx > 0) + int kkk = 1; + int flag = pt3D->nPointIdx & 0xffff; + if (flag > 0) + { + rgb = objColor[flag % 8]; // { 255, 97, 0 }; + size = 1; + } + 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; + } + } + + int linePtNum = (int)objects.size(); + sw << "Line_" << lineNum << "_0_" << linePtNum + 1 << std::endl; + lineNum++; + for (int i = 0; i < linePtNum; i++) + { + if (i %2 == 0) + { + size = 10; + rgb = { 255, 0, 0 }; + } + else + { + rgb = { 255, 255, 0 }; + size = 10; + } + //int colorIdx = objects[i].nPointIdx % 8; + //rgb = objColor[colorIdx]; + float x = (float)objects[i].pt3D.x; + float y = (float)objects[i].pt3D.y; + float z = (float)objects[i].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 }; + size = 2; + for (int i = 0; i < objNumber; i++) + { + if (abs(workpiecePositions[i].y_dir.z) > 1e-4) + { + SVzNL3DPoint dirPt; + dirPt = { workpiecePositions[i].center.x + workpiecePositions[i].y_dir.x * 10, + workpiecePositions[i].center.y + workpiecePositions[i].y_dir.y * 10, + workpiecePositions[i].center.z + workpiecePositions[i].y_dir.z * 10 }; + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << workpiecePositions[i].center.x << "," << workpiecePositions[i].center.y << "," << workpiecePositions[i].center.z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << dirPt.x << "," << dirPt.y << "," << dirPt.z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + lineIdx++; + + dirPt = { workpiecePositions[i].center.x + workpiecePositions[i].z_dir.x * 10, + workpiecePositions[i].center.y + workpiecePositions[i].z_dir.y * 10, + workpiecePositions[i].center.z + workpiecePositions[i].z_dir.z * 10 }; + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << workpiecePositions[i].center.x << "," << workpiecePositions[i].center.y << "," << workpiecePositions[i].center.z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << dirPt.x << "," << dirPt.y << "," << dirPt.z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + lineIdx++; + } + } + + sw.close(); +} + +void _outputRGBDResult_XYZ_RGBD_binInfo( + char* fileName, + std::vector>& scanLines, + WD_HRM_BinInfo& poseInfo) +{ + int objNumber = 0; + if (poseInfo.center.z > 1e-4) + objNumber = 1; + + int lineNum = (int)scanLines.size(); + std::ofstream sw(fileName); + int realLines = (objNumber == 0) ? lineNum : (lineNum + 1); + 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 = (int)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]; + if (pt3D->nPointIdx > 0) + int kkk = 1; + int flag = pt3D->nPointIdx & 0xffff; + if (flag > 0) + { + rgb = objColor[flag % 8]; // { 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 (poseInfo.center.z > 1e-4) + { + sw << "Line_" << lineNum << "_0_1" << std::endl; + lineNum++; + size = 20; + rgb = { 255, 255, 0 }; + float x = (float)poseInfo.center.x; + float y = (float)poseInfo.center.y; + float z = (float)poseInfo.center.z; + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; + + //输出方向线条 + rgb = { 250, 0, 0 }; + size = 2; + double dirLen = 500; + SVzNL3DPoint dirPt1 = { poseInfo.center.x + poseInfo.x_dir.x * dirLen, + poseInfo.center.y + poseInfo.x_dir.y * dirLen, + poseInfo.center.z + poseInfo.x_dir.z * dirLen }; + SVzNL3DPoint dirPt2 = { poseInfo.center.x - poseInfo.x_dir.x * dirLen, + poseInfo.center.y - poseInfo.x_dir.y * dirLen, + poseInfo.center.z - poseInfo.x_dir.z * dirLen }; + + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << dirPt1.x << "," << dirPt1.y << "," << dirPt1.z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << dirPt2.x << "," << dirPt2.y << "," << dirPt2.z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + lineIdx++; + + SVzNL3DPoint pose_y = poseInfo.y_dir; + rgb = { 0, 250, 0 }; + SVzNL3DPoint dirPt; + dirPt = { poseInfo.center.x + pose_y.x * dirLen, + poseInfo.center.y + pose_y.y * dirLen, + poseInfo.center.z + pose_y.z * dirLen }; + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << poseInfo.center.x << "," << poseInfo.center.y << "," << poseInfo.center.z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << dirPt.x << "," << dirPt.y << "," << dirPt.z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + lineIdx++; + + rgb = { 0, 0, 250 }; + dirPt1 = { poseInfo.center.x + poseInfo.bottomNormal.x * dirLen, + poseInfo.center.y + poseInfo.bottomNormal.y * dirLen, + poseInfo.center.z + poseInfo.bottomNormal.z * dirLen }; + dirPt2 = { poseInfo.center.x - poseInfo.bottomNormal.x * dirLen, + poseInfo.center.y - poseInfo.bottomNormal.y * dirLen, + poseInfo.center.z - poseInfo.bottomNormal.z * dirLen }; + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << dirPt1.x << "," << dirPt1.y << "," << dirPt1.z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << dirPt2.x << "," << dirPt2.y << "," << dirPt2.z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + lineIdx++; + + //输出外接矩形 + dirPt1 = poseInfo.minRectVertex[0]; + dirPt2 = poseInfo.minRectVertex[1]; + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << dirPt1.x << "," << dirPt1.y << "," << dirPt1.z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << dirPt2.x << "," << dirPt2.y << "," << dirPt2.z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + lineIdx++; + + dirPt1 = poseInfo.minRectVertex[1]; + dirPt2 = poseInfo.minRectVertex[2]; + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << dirPt1.x << "," << dirPt1.y << "," << dirPt1.z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << dirPt2.x << "," << dirPt2.y << "," << dirPt2.z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + lineIdx++; + + dirPt1 = poseInfo.minRectVertex[2]; + dirPt2 = poseInfo.minRectVertex[3]; + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << dirPt1.x << "," << dirPt1.y << "," << dirPt1.z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << dirPt2.x << "," << dirPt2.y << "," << dirPt2.z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + lineIdx++; + + dirPt1 = poseInfo.minRectVertex[3]; + dirPt2 = poseInfo.minRectVertex[0]; + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << dirPt1.x << "," << dirPt1.y << "," << dirPt1.z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << dirPt2.x << "," << dirPt2.y << "," << dirPt2.z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + lineIdx++; + } + + sw.close(); +} + +void _outputRGBDResult_XYZ_RGBD_workpieceInfo( + char* fileName, + std::vector>& scanLines, + std::vector& poseInfo) +{ + int objNumber = 0; + if (poseInfo.size()>0) + objNumber = 1; + + int lineNum = (int)scanLines.size(); + std::ofstream sw(fileName); + int realLines = (objNumber == 0) ? lineNum : (lineNum + 1); + 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 = (int)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]; + if (pt3D->nPointIdx > 0) + int kkk = 1; + int flag = pt3D->nPointIdx & 0xffff; + if (flag > 0) + { + rgb = objColor[flag % 8]; // { 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 (poseInfo.size() > 1e-4) + { + int objNum = (int)poseInfo.size(); + sw << "Line_" << lineNum << "_0_" << objNum << std::endl; + lineNum++; + size = 20; + rgb = { 255, 255, 0 }; + for (int m = 0; m < (int)poseInfo.size(); m++) + { + float x = (float)poseInfo[m].center.x; + float y = (float)poseInfo[m].center.y; + float z = (float)poseInfo[m].center.z; + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; + } + //输出方向线条 + rgb = { 250, 0, 0 }; + size = 2; + double dirLen = 500; + for (int m = 0; m < (int)poseInfo.size(); m++) + { + SVzNL3DPoint dirPt1 = { poseInfo[m].center.x + poseInfo[m].z_dir.x * dirLen, + poseInfo[m].center.y + poseInfo[m].z_dir.y * dirLen, + poseInfo[m].center.z + poseInfo[m].z_dir.z * dirLen }; + SVzNL3DPoint dirPt2 = { poseInfo[m].center.x - poseInfo[m].z_dir.x * dirLen, + poseInfo[m].center.y - poseInfo[m].z_dir.y * dirLen, + poseInfo[m].center.z - poseInfo[m].z_dir.z * dirLen }; + + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << dirPt1.x << "," << dirPt1.y << "," << dirPt1.z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << dirPt2.x << "," << dirPt2.y << "," << dirPt2.z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + lineIdx++; + } + } + + sw.close(); +} + +void _outputRGBDResult_XYZ_RGBD_planningResult( + char* fileName, + std::vector>& scanLines, + WD_HRM_BinInfo& binInfo, + std::vector& posInfo, + const double workpieceHeight) +{ + int objNumber = 0; + if (posInfo.size() > 0) + objNumber = 1; + + int lineNum = (int)scanLines.size(); + std::ofstream sw(fileName); + int realLines = (objNumber == 0) ? lineNum : (lineNum + 1); + 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 = (int)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]; + if (pt3D->nPointIdx > 0) + int kkk = 1; + int flag = pt3D->nPointIdx & 0xffff; + if (flag > 0) + { + rgb = objColor[flag % 8]; // { 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 (posInfo.size() > 0) + { + int num = (int)posInfo.size(); + sw << "Line_" << lineNum << "_0_" << num << std::endl; + lineNum++; + size = 20; + rgb = { 255, 255, 0 }; + for(int m = 0; m >& 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(); +} + +void _outputWorkpieceInfo(char* fileName, std::vector< WD_workpieceInfo>& workpiecePositions) +{ + std::ofstream sw(fileName); + char dataStr[250]; + + int number = (int)workpiecePositions.size(); + for (int i = 0; i < number; i++) + { + sprintf_s(dataStr, 250, "工件_%d", i + 1); + sw << dataStr << std::endl; + + sprintf_s(dataStr, 50, " center: (%g, %g, %g)", workpiecePositions[i].center.x, workpiecePositions[i].center.y, workpiecePositions[i].center.z); + sw << dataStr << std::endl; + sprintf_s(dataStr, 50, " x_dir: (%g, %g, %g)", workpiecePositions[i].x_dir.x, workpiecePositions[i].x_dir.y, workpiecePositions[i].x_dir.z); + sw << dataStr << std::endl; + sprintf_s(dataStr, 50, " y_dir: (%g, %g, %g)", workpiecePositions[i].y_dir.x, workpiecePositions[i].y_dir.y, workpiecePositions[i].y_dir.z); + sw << dataStr << std::endl; + sprintf_s(dataStr, 50, " z_dir: (%g, %g, %g)", workpiecePositions[i].z_dir.x, workpiecePositions[i].z_dir.y, workpiecePositions[i].z_dir.z); + sw << dataStr << std::endl; + } + sw.close(); +} + +void _outputBinInfo(char* fileName, WD_HRM_BinInfo& binInfo) +{ + std::ofstream sw(fileName); + char dataStr[250]; + + sprintf_s(dataStr, 50, " center: (%g, %g, %g)", binInfo.center.x, binInfo.center.y, binInfo.center.z); + sw << dataStr << std::endl; + sprintf_s(dataStr, 50, " L: %g; W: %g", binInfo.length, binInfo.width); + sw << dataStr << std::endl; + sprintf_s(dataStr, 50, " topZ: %g", binInfo.binTopZ); + sw << dataStr << std::endl; + sprintf_s(dataStr, 50, " bottom_dir: (%g, %g, %g)", binInfo.bottomNormal.x, binInfo.bottomNormal.y, binInfo.bottomNormal.z); + sw << dataStr << std::endl; + sprintf_s(dataStr, 50, " x_dir: (%g, %g, %g)", binInfo.x_dir.x, binInfo.x_dir.y, binInfo.x_dir.z); + sw << dataStr << std::endl; + sprintf_s(dataStr, 50, " y_dir: (%g, %g, %g)", binInfo.y_dir.x, binInfo.y_dir.y, binInfo.y_dir.z); + sw << dataStr << std::endl; + for (int i = 0; i < 4; i++) + { + sprintf_s(dataStr, 50, " vertex: (%g, %g, %g)", binInfo.minRectVertex[i].x, binInfo.minRectVertex[i].y, binInfo.minRectVertex[i].z); + sw << dataStr << std::endl; + } + sw.close(); +} + +WD_HRM_BinInfo _readBinInfo(char* fileName) +{ + //设置初始结果 + WD_HRM_BinInfo binInfo; + memset(&binInfo, 0, sizeof(WD_HRM_BinInfo)); + + std::ifstream inputFile(fileName); + std::string linedata; + + if (inputFile.is_open() == false) + return binInfo; + + float x, y, z; + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), " center: (%f, %f, %f)", &x, &y, &z); + binInfo.center = { x, y, z }; + + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), " L: %lf; W: %lf", &binInfo.length, &binInfo.width); + + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), " topZ: %lf", &binInfo.binTopZ); + + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), " bottom_dir: (%lf, %lf, %lf)", &binInfo.bottomNormal.x, &binInfo.bottomNormal.y, &binInfo.bottomNormal.z); + + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), " x_dir: (%lf, %lf, %lf)", &binInfo.x_dir.x, &binInfo.x_dir.y, &binInfo.x_dir.z); + + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), " y_dir: (%lf, %lf, %lf)", &binInfo.y_dir.x, &binInfo.y_dir.y, &binInfo.y_dir.z); + + for (int i = 0; i < 4; i++) + { + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), " vertex: (%f, %f, %f)", &x, &y, &z); + binInfo.minRectVertex[i] = { x, y, z }; + } + inputFile.close(); + return binInfo; +} + +void _outputWorkpieceInfo(char* fileName, WD_HRM_workpieceSizeInfo& workpieceInfo) +{ + std::ofstream sw(fileName); + char dataStr[250]; + + sprintf_s(dataStr, 50, " center: (%g, %g, %g)", workpieceInfo.center.x, workpieceInfo.center.y, workpieceInfo.center.z); + sw << dataStr << std::endl; + sprintf_s(dataStr, 50, " R: %g; H: %g", workpieceInfo.workpieceRadius, workpieceInfo.workpieceHeight); + sw << dataStr << std::endl; + sprintf_s(dataStr, 50, " layerZ: %g", workpieceInfo.layerZValue); + sw << dataStr << std::endl; + + sw.close(); +} + +WD_HRM_workpieceSizeInfo _readWorkpieceInfo(char* fileName) +{ + //设置初始结果 + WD_HRM_workpieceSizeInfo workpieceInfo; + memset(&workpieceInfo, 0, sizeof(WD_HRM_workpieceSizeInfo)); + + std::ifstream inputFile(fileName); + std::string linedata; + + if (inputFile.is_open() == false) + return workpieceInfo; + + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), " center: (%lf, %lf, %lf)", &workpieceInfo.center.x, &workpieceInfo.center.y, &workpieceInfo.center.z); + + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), " R: %lf; H: %lf", &workpieceInfo.workpieceRadius, &workpieceInfo.workpieceHeight); + + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), " layerZ: %lf", &workpieceInfo.layerZValue); + + inputFile.close(); + return workpieceInfo; +} + +void _XOYprojection_XYZUV( + cv::Mat& img, + std::vector>& dataLines, + std::vector< WD_workpieceInfo>& holes, + const double scale, + const int sideWidth, + const SVzNLRangeD x_range, + const SVzNLRangeD y_range, + bool drawDirAngle, + const double dirAngleLen) +{ + 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< SVzNLPositionD>& a_line = dataLines[line]; + for (int i = 0; i < a_line.size(); i++) + { + SVzNLPositionD& pt3D = a_line[i]; + if (pt3D.pt3D.z < 1e-4) + continue; + + int flag = pt3D.nPointIdx & 0xffff; + if (flag > 0) + { + rgb = objColor[flag % 8]; // { 255, 97, 0 }; + size = 1; + } + else + { + rgb = { 150, 150, 150 }; + size = 1; + } + + double x = pt3D.pt3D.x; + double y = pt3D.pt3D.y; + int px = (int)((x - x_range.min) / scale + sideWidth); + int py = (int)((y - y_range.min) / scale + sideWidth); + 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 (holes.size() > 0) + { + for (int i = 0; i < (int)holes.size(); i++) + { + if (i == 0) + { + rgb = { 255, 0, 0 }; + size = int(3.0 / scale); + } + else + { + rgb = { 255, 255, 0 }; + size = int(3.0 / scale); + } + WD_workpieceInfo& a_hole = holes[i]; + int px = (int)((a_hole.center.x - x_range.min) / scale + sideWidth); + int py = (int)((a_hole.center.y - y_range.min) / scale + sideWidth); + cv::circle(img, cv::Point(px, py), size, cv::Scalar(rgb[2], rgb[1], rgb[0]), -1); + rgb = { 0, 255, 0 }; + size = int(2.0 / scale); + if (true == drawDirAngle) + { + //画线 + double x1 = a_hole.center.x; + double y1 = a_hole.center.y; + int px1 = (int)((x1 - x_range.min) / scale + sideWidth); + int py1 = (int)((y1 - y_range.min) / scale + sideWidth); + //x轴 + double x2 = a_hole.center.x + dirAngleLen * a_hole.x_dir.x; + double y2 = a_hole.center.y + dirAngleLen * a_hole.x_dir.y; + int px2 = (int)((x2 - x_range.min) / scale + sideWidth); + int py2 = (int)((y2 - y_range.min) / scale + sideWidth); + rgb = { 255, 0, 0 }; //x轴用红色 + cv::line(img, cv::Point(px1, py1), cv::Point(px2, py2), cv::Scalar(rgb[2], rgb[1], rgb[0]), 1); + //y轴 + x2 = a_hole.center.x + dirAngleLen * a_hole.y_dir.x; + y2 = a_hole.center.y + dirAngleLen * a_hole.y_dir.y; + px2 = (int)((x2 - x_range.min) / scale + sideWidth); + py2 = (int)((y2 - y_range.min) / scale + sideWidth); + rgb = { 0, 255, 0 }; //y轴用绿色 + cv::line(img, cv::Point(px1, py1), cv::Point(px2, py2), cv::Scalar(rgb[2], rgb[1], rgb[0]), 1); + //z轴 + x2 = a_hole.center.x + dirAngleLen * a_hole.z_dir.x; + y2 = a_hole.center.y + dirAngleLen * a_hole.z_dir.y; + px2 = (int)((x2 - x_range.min) / scale + sideWidth); + py2 = (int)((y2 - y_range.min) / scale + sideWidth); + rgb = { 0, 0, 255 }; //z轴用蓝色 + cv::line(img, cv::Point(px1, py1), cv::Point(px2, py2), cv::Scalar(rgb[2], rgb[1], rgb[0]), 1); + } + } + } +} + +void EulerRpyToRotation1(const double rpy[3], double matrix3d[9]) { + double cos0 = cos(rpy[0] * PI / 180); + double sin0 = sin(rpy[0] * PI / 180); + double cos1 = cos(rpy[1] * PI / 180); + double sin1 = sin(rpy[1] * PI / 180); + double cos2 = cos(rpy[2] * PI / 180); + double sin2 = sin(rpy[2] * PI / 180); + matrix3d[0] = cos2 * cos1; + matrix3d[1] = cos2 * sin1 * sin0 - sin2 * cos0; + matrix3d[2] = cos2 * sin1 * cos0 + sin2 * sin0; + matrix3d[3] = sin2 * cos1; + matrix3d[4] = sin2 * sin1 * sin0 + cos2 * cos0; + matrix3d[5] = sin2 * sin1 * cos0 - cos2 * sin0; + matrix3d[6] = -sin1; + matrix3d[7] = cos1 * sin0; + matrix3d[8] = cos1 * cos0; + return; +} + +void _rotateCloudPts_XYZUV( + std::vector>& scanLines, + double matrix3d[9], + std::vector>& rotateLines, + SVzNLRangeD& rx_range, SVzNLRangeD& ry_range) +{ + int lineNum = (int)scanLines.size(); + rotateLines.resize(lineNum); + rx_range.min = 0; + rx_range.max = -1; + ry_range.min = 0; + ry_range.max = -1; + for (int line = 0; line < lineNum; line++) + { + for (int i = 0; i < (int)scanLines[line].size(); i++) + { + SVzNLPositionD& pt3D = scanLines[line][i]; + if (pt3D.pt3D.z < 1e-4) + continue; + + SVzNLPositionD r_pt; + r_pt.pt3D = _ptRotate(pt3D.pt3D, matrix3d); + r_pt.nPointIdx = pt3D.nPointIdx; + r_pt.ptLeft2D = pt3D.ptLeft2D; + r_pt.ptRight2D = pt3D.ptRight2D; + rotateLines[line].push_back(r_pt); + if (rx_range.max < rx_range.min) + { + rx_range.min = r_pt.pt3D.x; + rx_range.max = r_pt.pt3D.x; + } + else + { + if (rx_range.min > r_pt.pt3D.x) + rx_range.min = r_pt.pt3D.x; + if (rx_range.max < r_pt.pt3D.x) + rx_range.max = r_pt.pt3D.x; + } + if (ry_range.max < ry_range.min) + { + ry_range.min = r_pt.pt3D.y; + ry_range.max = r_pt.pt3D.y; + } + else + { + if (ry_range.min > r_pt.pt3D.y) + ry_range.min = r_pt.pt3D.y; + if (ry_range.max < r_pt.pt3D.y) + ry_range.max = r_pt.pt3D.y; + } + } + } +} + +void _genXOYProjectionImage_XYZUV( + cv::String& fileName, + std::vector>& scanLines, + const double scale, + std::vector< WD_workpieceInfo>& holes, + const double rpy[3], const double dirLen) +{ + //旋转视角显示 + double matrix3d[9]; + EulerRpyToRotation1(rpy, matrix3d); + std::vector r_objOps; + r_objOps.resize(holes.size()); + std::vector> rotateLines; + SVzNLRangeD rx_range, ry_range; + _rotateCloudPts_XYZUV(scanLines, matrix3d, rotateLines, rx_range, ry_range); + + //统计X和Y的范围 + int x_cols = (int)((rx_range.max - rx_range.min) / scale) + 1; + if (x_cols % 2 == 1) + x_cols += 1; + int y_rows = (int)((ry_range.max - ry_range.min) / scale) + 1; + if (y_rows % 2 == 1) + y_rows += 1; + + int sideWidth = 4; + int imgCols = x_cols + sideWidth * 2; + int imgRows = y_rows + sideWidth * 2; + + cv::Mat img = cv::Mat::zeros(imgRows, imgCols, CV_8UC3); + //计算投影比例 + _XOYprojection_XYZUV(img, rotateLines, r_objOps, scale, sideWidth, rx_range, ry_range, true, dirLen); + cv::imwrite(fileName, img); + return; +} + +void _rotateCloudPts( + std::vector>& scanLines, + double matrix3d[9], + std::vector>& rotateLines, + SVzNLRangeD& rx_range, SVzNLRangeD& ry_range) +{ + int lineNum = (int)scanLines.size(); + rotateLines.resize(lineNum); + rx_range.min = 0; + rx_range.max = -1; + ry_range.min = 0; + ry_range.max = -1; + for (int line = 0; line < lineNum; line++) + { + for (int i = 0; i < (int)scanLines[line].size(); i++) + { + SVzNL3DPosition& pt3D = scanLines[line][i]; + if (pt3D.pt3D.z < 1e-4) + continue; + + SVzNL3DPosition r_pt; + r_pt.pt3D = _ptRotate(pt3D.pt3D, matrix3d); + r_pt.nPointIdx = pt3D.nPointIdx; + rotateLines[line].push_back(r_pt); + if (rx_range.max < rx_range.min) + { + rx_range.min = r_pt.pt3D.x; + rx_range.max = r_pt.pt3D.x; + } + else + { + if (rx_range.min > r_pt.pt3D.x) + rx_range.min = r_pt.pt3D.x; + if (rx_range.max < r_pt.pt3D.x) + rx_range.max = r_pt.pt3D.x; + } + if (ry_range.max < ry_range.min) + { + ry_range.min = r_pt.pt3D.y; + ry_range.max = r_pt.pt3D.y; + } + else + { + if (ry_range.min > r_pt.pt3D.y) + ry_range.min = r_pt.pt3D.y; + if (ry_range.max < r_pt.pt3D.y) + ry_range.max = r_pt.pt3D.y; + } + } + } +} + +void _XOYprojection_binInfo( + cv::Mat& img, + std::vector>& dataLines, + WD_HRM_BinInfo& binInfo, + const double scale, + const int sideWidth, + const SVzNLRangeD x_range, + const SVzNLRangeD y_range, + const double dirAngleLen) +{ + 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< SVzNL3DPosition>& a_line = dataLines[line]; + for (int i = 0; i < a_line.size(); i++) + { + SVzNL3DPosition& pt3D = a_line[i]; + if (pt3D.pt3D.z < 1e-4) + continue; + + int flag = pt3D.nPointIdx & 0xffff; + if (flag > 0) + { + rgb = objColor[flag % 8]; // { 255, 97, 0 }; + size = 1; + } + else + { + rgb = { 150, 150, 150 }; + size = 1; + } + + double x = pt3D.pt3D.x; + double y = pt3D.pt3D.y; + int px = (int)((x - x_range.min) / scale + sideWidth); + int py = (int)((y - y_range.min) / scale + sideWidth); + 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 (binInfo.center.z > 1e-4) + { + rgb = { 255, 0, 0 }; + size = int(10.0 / scale); + int px = (int)((binInfo.center.x - x_range.min) / scale + sideWidth); + int py = (int)((binInfo.center.y - y_range.min) / scale + sideWidth); + cv::circle(img, cv::Point(px, py), size, cv::Scalar(rgb[2], rgb[1], rgb[0]), -1); + //画线 + double x1 = binInfo.center.x; + double y1 = binInfo.center.y; + int px1 = (int)((x1 - x_range.min) / scale + sideWidth); + int py1 = (int)((y1 - y_range.min) / scale + sideWidth); + //x轴 + double mx2 = binInfo.center.x - dirAngleLen * binInfo.x_dir.x; + double my2 = binInfo.center.y - dirAngleLen * binInfo.x_dir.y; + int mpx2 = (int)((mx2 - x_range.min) / scale + sideWidth); + int mpy2 = (int)((my2 - y_range.min) / scale + sideWidth); + + double x2 = binInfo.center.x + dirAngleLen * binInfo.x_dir.x; + double y2 = binInfo.center.y + dirAngleLen * binInfo.x_dir.y; + int px2 = (int)((x2 - x_range.min) / scale + sideWidth); + int py2 = (int)((y2 - y_range.min) / scale + sideWidth); + rgb = { 255, 0, 0 }; //x轴用红色 + cv::line(img, cv::Point(mpx2, mpy2), cv::Point(px2, py2), cv::Scalar(rgb[2], rgb[1], rgb[0]), 1); + //y轴 + x2 = binInfo.center.x + dirAngleLen * binInfo.y_dir.x; + y2 = binInfo.center.y + dirAngleLen * binInfo.y_dir.y; + px2 = (int)((x2 - x_range.min) / scale + sideWidth); + py2 = (int)((y2 - y_range.min) / scale + sideWidth); + rgb = { 0, 255, 0 }; //y轴用绿色 + cv::line(img, cv::Point(px1, py1), cv::Point(px2, py2), cv::Scalar(rgb[2], rgb[1], rgb[0]), 1); + + //边1 + x1 = binInfo.minRectVertex[0].x; + y1 = binInfo.minRectVertex[0].y; + px1 = (int)((x1 - x_range.min) / scale + sideWidth); + py1 = (int)((y1 - y_range.min) / scale + sideWidth); + x2 = binInfo.minRectVertex[1].x; + y2 = binInfo.minRectVertex[1].y; + px2 = (int)((x2 - x_range.min) / scale + sideWidth); + py2 = (int)((y2 - y_range.min) / scale + sideWidth); + rgb = { 0, 0, 255 }; //y轴用绿色 + cv::line(img, cv::Point(px1, py1), cv::Point(px2, py2), cv::Scalar(rgb[2], rgb[1], rgb[0]), 2); + //边2 + x1 = binInfo.minRectVertex[1].x; + y1 = binInfo.minRectVertex[1].y; + px1 = (int)((x1 - x_range.min) / scale + sideWidth); + py1 = (int)((y1 - y_range.min) / scale + sideWidth); + x2 = binInfo.minRectVertex[2].x; + y2 = binInfo.minRectVertex[2].y; + px2 = (int)((x2 - x_range.min) / scale + sideWidth); + py2 = (int)((y2 - y_range.min) / scale + sideWidth); + rgb = { 0, 0, 255 }; //y轴用绿色 + cv::line(img, cv::Point(px1, py1), cv::Point(px2, py2), cv::Scalar(rgb[2], rgb[1], rgb[0]), 2); + //边3 + x1 = binInfo.minRectVertex[2].x; + y1 = binInfo.minRectVertex[2].y; + px1 = (int)((x1 - x_range.min) / scale + sideWidth); + py1 = (int)((y1 - y_range.min) / scale + sideWidth); + x2 = binInfo.minRectVertex[3].x; + y2 = binInfo.minRectVertex[3].y; + px2 = (int)((x2 - x_range.min) / scale + sideWidth); + py2 = (int)((y2 - y_range.min) / scale + sideWidth); + rgb = { 0, 0, 255 }; //y轴用绿色 + cv::line(img, cv::Point(px1, py1), cv::Point(px2, py2), cv::Scalar(rgb[2], rgb[1], rgb[0]), 2); + //边4 + x1 = binInfo.minRectVertex[3].x; + y1 = binInfo.minRectVertex[3].y; + px1 = (int)((x1 - x_range.min) / scale + sideWidth); + py1 = (int)((y1 - y_range.min) / scale + sideWidth); + x2 = binInfo.minRectVertex[0].x; + y2 = binInfo.minRectVertex[0].y; + px2 = (int)((x2 - x_range.min) / scale + sideWidth); + py2 = (int)((y2 - y_range.min) / scale + sideWidth); + rgb = { 0, 0, 255 }; //y轴用绿色 + cv::line(img, cv::Point(px1, py1), cv::Point(px2, py2), cv::Scalar(rgb[2], rgb[1], rgb[0]), 2); + } + +} + +void _genXOYProjectionImage_binInfo( + cv::String& fileName, + std::vector>& scanLines, + const double scale, + WD_HRM_BinInfo& binInfo, + const double rpy[3], const double dirLen) +{ + //旋转视角显示 + double matrix3d[9]; + EulerRpyToRotation1(rpy, matrix3d); + WD_HRM_BinInfo rotateBin; + rotateBin.length = binInfo.length; + rotateBin.width = binInfo.width; + rotateBin.center = _ptRotate(binInfo.center, matrix3d); + rotateBin.bottomNormal = _ptRotate(binInfo.bottomNormal, matrix3d); + rotateBin.x_dir = _ptRotate(binInfo.x_dir, matrix3d); + rotateBin.y_dir = _ptRotate(binInfo.y_dir, matrix3d); + for (int i = 0; i < 4; i++) + rotateBin.minRectVertex[i] = _ptRotate(binInfo.minRectVertex[i], matrix3d); + + std::vector> rotateLines; + SVzNLRangeD rx_range, ry_range; + _rotateCloudPts(scanLines, matrix3d, rotateLines, rx_range, ry_range); + + //统计X和Y的范围 + int x_cols = (int)((rx_range.max - rx_range.min) / scale) + 1; + if (x_cols % 2 == 1) + x_cols += 1; + int y_rows = (int)((ry_range.max - ry_range.min) / scale) + 1; + if (y_rows % 2 == 1) + y_rows += 1; + + int sideWidth = 32; + int imgCols = x_cols + sideWidth * 2; + int imgRows = y_rows + sideWidth * 2; + + cv::Mat img = cv::Mat::zeros(imgRows, imgCols, CV_8UC3); + //计算投影比例 + _XOYprojection_binInfo(img, rotateLines, rotateBin, scale, sideWidth, rx_range, ry_range, dirLen); + cv::imwrite(fileName, img); + return; +} + +void _XOYprojection_workpieceInfo( + cv::Mat& img, + std::vector>& dataLines, + std::vector& workpieceInfo, + const double scale, + const int sideWidth, + const SVzNLRangeD x_range, + const SVzNLRangeD y_range, + const double dirAngleLen) +{ + 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< SVzNL3DPosition>& a_line = dataLines[line]; + for (int i = 0; i < a_line.size(); i++) + { + SVzNL3DPosition& pt3D = a_line[i]; + if (pt3D.pt3D.z < 1e-4) + continue; + + int flag = pt3D.nPointIdx & 0xffff; + if (flag > 0) + { + rgb = objColor[flag % 8]; // { 255, 97, 0 }; + size = 1; + } + else + { + rgb = { 150, 150, 150 }; + size = 1; + } + + double x = pt3D.pt3D.x; + double y = pt3D.pt3D.y; + int px = (int)((x - x_range.min) / scale + sideWidth); + int py = (int)((y - y_range.min) / scale + sideWidth); + 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 (workpieceInfo.size() > 0) + { + + for (int i = 0; i < (int)workpieceInfo.size(); i++) + { + rgb = { 255, 0, 0 }; + size = int(10.0 / scale); + int px = (int)((workpieceInfo[i].center.x - x_range.min) / scale + sideWidth); + int py = (int)((workpieceInfo[i].center.y - y_range.min) / scale + sideWidth); + cv::circle(img, cv::Point(px, py), size, cv::Scalar(rgb[2], rgb[1], rgb[0]), -1); + //画圆 + rgb = { 0, 0, 255 }; + size = (int)(workpieceInfo[i].value / scale); + cv::circle(img, cv::Point(px, py), size, cv::Scalar(rgb[2], rgb[1], rgb[0]), 2); + } + } +} + +void _genXOYProjectionImage_workpieceInfo( + cv::String& fileName, + std::vector>& scanLines, + const double scale, + std::vector& workpieceInfo, + const double rpy[3], const double dirLen) +{ + //旋转视角显示 + double matrix3d[9]; + EulerRpyToRotation1(rpy, matrix3d); + + std::vector rotatedWorkpiece; + for (int i = 0; i < (int)workpieceInfo.size(); i++) + { + WD_workpieceInfo rotateObj; + rotateObj.workpieceType = workpieceInfo[i].workpieceType; + rotateObj.value = workpieceInfo[i].value; + rotateObj.center = _ptRotate(workpieceInfo[i].center, matrix3d); + rotateObj.z_dir = _ptRotate(workpieceInfo[i].z_dir, matrix3d); + rotateObj.x_dir = _ptRotate(workpieceInfo[i].x_dir, matrix3d); + rotateObj.y_dir = _ptRotate(workpieceInfo[i].y_dir, matrix3d); + rotatedWorkpiece.push_back(rotateObj); + } + + std::vector> rotateLines; + SVzNLRangeD rx_range, ry_range; + _rotateCloudPts(scanLines, matrix3d, rotateLines, rx_range, ry_range); + + //统计X和Y的范围 + int x_cols = (int)((rx_range.max - rx_range.min) / scale) + 1; + if (x_cols % 2 == 1) + x_cols += 1; + int y_rows = (int)((ry_range.max - ry_range.min) / scale) + 1; + if (y_rows % 2 == 1) + y_rows += 1; + + int sideWidth = 32; + int imgCols = x_cols + sideWidth * 2; + int imgRows = y_rows + sideWidth * 2; + + cv::Mat img = cv::Mat::zeros(imgRows, imgCols, CV_8UC3); + //计算投影比例 + _XOYprojection_workpieceInfo(img, rotateLines, rotatedWorkpiece, scale, sideWidth, rx_range, ry_range, dirLen); + cv::imwrite(fileName, img); + return; +} + +void _XOYprojection_planningResult( + cv::Mat& img, + std::vector>& dataLines, + std::vector& workpieceInfo, + WD_HRM_BinInfo& binInfo, + const double scale, + const int sideWidth, + const SVzNLRangeD x_range, + const SVzNLRangeD y_range, + const double dirAngleLen) +{ + 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< SVzNL3DPosition>& a_line = dataLines[line]; + for (int i = 0; i < a_line.size(); i++) + { + SVzNL3DPosition& pt3D = a_line[i]; + if (pt3D.pt3D.z < 1e-4) + continue; + + int flag = pt3D.nPointIdx & 0xffff; + if (flag > 0) + { + rgb = objColor[flag % 8]; // { 255, 97, 0 }; + size = 1; + } + else + { + rgb = { 150, 150, 150 }; + size = 1; + } + + double x = pt3D.pt3D.x; + double y = pt3D.pt3D.y; + int px = (int)((x - x_range.min) / scale + sideWidth); + int py = (int)((y - y_range.min) / scale + sideWidth); + 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 (workpieceInfo.size() > 0) + { + for (int i = 0; i < (int)workpieceInfo.size(); i++) + { + rgb = { 255, 0, 0 }; + size = int(10.0 / scale); + int px = (int)((workpieceInfo[i].center.x - x_range.min) / scale + sideWidth); + int py = (int)((workpieceInfo[i].center.y - y_range.min) / scale + sideWidth); + cv::circle(img, cv::Point(px, py), size, cv::Scalar(rgb[2], rgb[1], rgb[0]), -1); + //画圆 + rgb = { 0, 0, 255 }; + size = (int)(workpieceInfo[i].value / scale); + cv::circle(img, cv::Point(px, py), size, cv::Scalar(rgb[2], rgb[1], rgb[0]), 2); + } + + //边1 + double x1 = binInfo.minRectVertex[0].x; + double y1 = binInfo.minRectVertex[0].y; + int px1 = (int)((x1 - x_range.min) / scale + sideWidth); + int py1 = (int)((y1 - y_range.min) / scale + sideWidth); + double x2 = binInfo.minRectVertex[1].x; + double y2 = binInfo.minRectVertex[1].y; + int px2 = (int)((x2 - x_range.min) / scale + sideWidth); + int py2 = (int)((y2 - y_range.min) / scale + sideWidth); + rgb = { 0, 0, 255 }; //y轴用绿色 + cv::line(img, cv::Point(px1, py1), cv::Point(px2, py2), cv::Scalar(rgb[2], rgb[1], rgb[0]), 2); + //边2 + x1 = binInfo.minRectVertex[1].x; + y1 = binInfo.minRectVertex[1].y; + px1 = (int)((x1 - x_range.min) / scale + sideWidth); + py1 = (int)((y1 - y_range.min) / scale + sideWidth); + x2 = binInfo.minRectVertex[2].x; + y2 = binInfo.minRectVertex[2].y; + px2 = (int)((x2 - x_range.min) / scale + sideWidth); + py2 = (int)((y2 - y_range.min) / scale + sideWidth); + rgb = { 0, 0, 255 }; //y轴用绿色 + cv::line(img, cv::Point(px1, py1), cv::Point(px2, py2), cv::Scalar(rgb[2], rgb[1], rgb[0]), 2); + //边3 + x1 = binInfo.minRectVertex[2].x; + y1 = binInfo.minRectVertex[2].y; + px1 = (int)((x1 - x_range.min) / scale + sideWidth); + py1 = (int)((y1 - y_range.min) / scale + sideWidth); + x2 = binInfo.minRectVertex[3].x; + y2 = binInfo.minRectVertex[3].y; + px2 = (int)((x2 - x_range.min) / scale + sideWidth); + py2 = (int)((y2 - y_range.min) / scale + sideWidth); + rgb = { 0, 0, 255 }; //y轴用绿色 + cv::line(img, cv::Point(px1, py1), cv::Point(px2, py2), cv::Scalar(rgb[2], rgb[1], rgb[0]), 2); + //边4 + x1 = binInfo.minRectVertex[3].x; + y1 = binInfo.minRectVertex[3].y; + px1 = (int)((x1 - x_range.min) / scale + sideWidth); + py1 = (int)((y1 - y_range.min) / scale + sideWidth); + x2 = binInfo.minRectVertex[0].x; + y2 = binInfo.minRectVertex[0].y; + px2 = (int)((x2 - x_range.min) / scale + sideWidth); + py2 = (int)((y2 - y_range.min) / scale + sideWidth); + rgb = { 0, 0, 255 }; //y轴用绿色 + cv::line(img, cv::Point(px1, py1), cv::Point(px2, py2), cv::Scalar(rgb[2], rgb[1], rgb[0]), 2); + + } +} + +void _genXOYProjectionImage_planningResult( + cv::String& fileName, + std::vector>& scanLines, + const double scale, + WD_HRM_BinInfo& binInfo, + std::vector& workpieceInfo, + const double rpy[3], const double dirLen) +{ + //旋转视角显示 + double matrix3d[9]; + EulerRpyToRotation1(rpy, matrix3d); + + std::vector rotatedWorkpiece; + for (int i = 0; i < (int)workpieceInfo.size(); i++) + { + WD_workpieceInfo rotateObj; + rotateObj.workpieceType = workpieceInfo[i].workpieceType; + rotateObj.value = workpieceInfo[i].value; + rotateObj.center = _ptRotate(workpieceInfo[i].center, matrix3d); + rotateObj.z_dir = _ptRotate(workpieceInfo[i].z_dir, matrix3d); + rotateObj.x_dir = _ptRotate(workpieceInfo[i].x_dir, matrix3d); + rotateObj.y_dir = _ptRotate(workpieceInfo[i].y_dir, matrix3d); + rotatedWorkpiece.push_back(rotateObj); + } + + std::vector> rotateLines; + SVzNLRangeD rx_range, ry_range; + _rotateCloudPts(scanLines, matrix3d, rotateLines, rx_range, ry_range); + + //统计X和Y的范围 + int x_cols = (int)((rx_range.max - rx_range.min) / scale) + 1; + if (x_cols % 2 == 1) + x_cols += 1; + int y_rows = (int)((ry_range.max - ry_range.min) / scale) + 1; + if (y_rows % 2 == 1) + y_rows += 1; + + int sideWidth = 32; + int imgCols = x_cols + sideWidth * 2; + int imgRows = y_rows + sideWidth * 2; + + cv::Mat img = cv::Mat::zeros(imgRows, imgCols, CV_8UC3); + //计算投影比例 + _XOYprojection_planningResult(img, rotateLines, rotatedWorkpiece, binInfo, scale, sideWidth, rx_range, ry_range, dirLen); + cv::imwrite(fileName, img); + return; +} + +//拓普发工件孔定位（工件定位) +#define HRM_TaperedWorkpiece_TEST_GROUP 1 +void HaiRuiMa_TaperedWorkpiece_test(void) +{ + const char* dataPath[HRM_TaperedWorkpiece_TEST_GROUP] = { + + "F:/ShangGu/项目/冠钦项目/宁波海瑞马/锥形工作2D+3D数据/数据1/", //0 + }; + + SVzNLRange fileIdx[HRM_TaperedWorkpiece_TEST_GROUP] = { + {1,21}, + }; + + const char* ver = wd_hybridPositioningVersion(); + printf("ver:%s\n", ver); + + for (int grp = 0; grp < HRM_TaperedWorkpiece_TEST_GROUP; grp++) + { + SSG_planeCalibPara groundCalibPara; + //初始化成单位阵 + groundCalibPara.planeCalib[0] = 1.0; + groundCalibPara.planeCalib[1] = 0.0; + groundCalibPara.planeCalib[2] = 0.0; + groundCalibPara.planeCalib[3] = 0.0; + groundCalibPara.planeCalib[4] = 1.0; + groundCalibPara.planeCalib[5] = 0.0; + groundCalibPara.planeCalib[6] = 0.0; + groundCalibPara.planeCalib[7] = 0.0; + groundCalibPara.planeCalib[8] = 1.0; + groundCalibPara.planeHeight = -1.0; + for (int i = 0; i < 9; i++) + groundCalibPara.invRMatrix[i] = groundCalibPara.planeCalib[i]; + char calibFile[250]; + sprintf_s(calibFile, "%sground_calib_para.txt", dataPath[grp]); + //groundCalibPara = _readCalibPara(calibFile); + + for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) + { + fidx =15; + char _scan_file[256]; + sprintf_s(_scan_file, "%s%d_cloud.txt", dataPath[grp], fidx); + std::vector> scanLines; + vzReadLaserScanPointFromFile_XYZUV_vector(_scan_file, scanLines); + _removeZeroLines_XYZUV(scanLines); + if (scanLines.size() == 0) + continue; + + sprintf_s(_scan_file, "%s%d_result.txt", dataPath[grp], fidx); + std::vector objROIs; + vzReadObj2DROI(_scan_file, objROIs); + + long t1 = (long)GetTickCount64();//统计时间 + + int errCode = 0; + std::vector< WD_workpieceInfo> workpiecePositions; + wd_HRM_TaperedWorkpiecePositioning( + scanLines, + objROIs, + groundCalibPara, + workpiecePositions, + &errCode); + long t2 = (long)GetTickCount64(); + if (errCode == SX_ERR_UNKNOWN_OBJECT) + printf("%s: %d(ms), 有异物残留!\n", _scan_file, (int)(t2 - t1)); + else if (errCode == SX_ERR_ZERO_OBJECTS) + printf("%s: %d(ms), 无产品!\n", _scan_file, (int)(t2 - t1)); + else + printf("%s: %d(ms), errCode=%d ...", _scan_file, (int)(t2 - t1), errCode); + //输出测试结果 + sprintf_s(_scan_file, "%sresult/LaserLine%d_result.txt", dataPath[grp], fidx); + _outputRGBDResult_XYZUV_RGBD(_scan_file, scanLines, workpiecePositions); + sprintf_s(calibFile, "%sresult/LaserLine%d_hole_info.txt", dataPath[grp], fidx); + _outputWorkpieceInfo(calibFile, workpiecePositions); + +#if 0 + sprintf_s(calibFile, "%sresult/LaserLine%d_holes_projection.png", dataPath[grp], fidx); + double rpy[3] = { -30, 15, 0 }; //{ 0,-45, 0 }; // + double angleDrawLen = 15; + double displayScale = 0.3; + cv::String imgName(calibFile); + _genXOYProjectionImage(imgName, scanLines, displayScale, workpiecePositions, rpy, angleDrawLen); +#endif + printf("done\n"); + } + } +} + +//地面调平 +#define HRM_BIN_TEST_GROUP 3 +void HaiRuiMa_groundCalib_test(void) +{ + const char* dataPath[HRM_BIN_TEST_GROUP] = { + + "F:/ShangGu/项目/冠钦项目/宁波海瑞马/码垛规划/数据2/工件1/", //0 + "F:/ShangGu/项目/冠钦项目/宁波海瑞马/码垛规划/数据2/工件2/", //1 + "F:/ShangGu/项目/冠钦项目/宁波海瑞马/码垛规划/数据2/工件3/", //2 + }; + + SVzNLRange fileIdx[HRM_BIN_TEST_GROUP] = { + {1,7}, {1,9}, {1,7}, + }; + + const char* ver = wd_hybridPositioningVersion(); + printf("ver:%s\n", ver); + + char _calib_datafile[256]; + sprintf_s(_calib_datafile, "F:/ShangGu/项目/冠钦项目/宁波海瑞马/码垛规划/数据2/地面数据.txt"); + int lineNum = 0; + float lineV = 0.0f; + int dataCalib = 0; + int maxTimeStamp = 0; + int clockPerSecond = 0; + std::vector> scanData; + vzReadLaserScanPointFromFile_XYZ_vector(_calib_datafile, scanData); + + lineNum = (int)scanData.size(); + if (scanData.size() > 0) + { + SSG_planeCalibPara calibPara = wd_getGroundCalibPara(scanData); + // + char calibFile[250]; + sprintf_s(calibFile, "F:/ShangGu/项目/冠钦项目/宁波海瑞马/码垛规划/数据2/ground_calib_para.txt"); + _outputCalibPara(calibFile, calibPara); +#if 1 + for (int grp = 0; grp < HRM_BIN_TEST_GROUP; grp++) + { + for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) + { + //fidx =4; + char _scan_file[256]; + sprintf_s(_scan_file, "%s隔板%d_LaserData_Jl26C177.txt", dataPath[grp], fidx); + std::vector> scanLines; + vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines); + _removeZeroLines_XYZ(scanLines); + if (scanLines.size() == 0) + continue; + lineNum = (int)scanLines.size(); + for (int i = 0; i < lineNum; i++) + wd_lineDataR(scanLines[i], calibPara.planeCalib, -1);//调平，去除地面 + sprintf_s(_scan_file, "%s隔板%d_LaserData_Jl26C177_ground_calibrated.txt", dataPath[grp], fidx); + int headNullLines = 0; + _outputScanDataFile_vector(_scan_file, scanLines, false, &headNullLines); + + sprintf_s(_scan_file, "%s工件%d_LaserData_Jl26C177.txt", dataPath[grp], fidx); + scanLines.clear(); + vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines); + _removeZeroLines_XYZ(scanLines); + if (scanLines.size() == 0) + continue; + lineNum = (int)scanLines.size(); + for (int i = 0; i < lineNum; i++) + wd_lineDataR(scanLines[i], calibPara.planeCalib, -1);//调平，去除地面 + sprintf_s(_scan_file, "%s工件%d_LaserData_Jl26C177_ground_calibrated.txt", dataPath[grp], fidx); + headNullLines = 0; + _outputScanDataFile_vector(_scan_file, scanLines, false, &headNullLines); + + printf("%s: calib done!\n", _scan_file); + } + } +#endif + printf("all calib done!\n", _calib_datafile); + } + +} +//海瑞马料筐尺寸计算 +void HaiRuiMa_BinSizeCompute_test(void) +{ + const char* dataPath[HRM_BIN_TEST_GROUP] = { + + "F:/ShangGu/项目/冠钦项目/宁波海瑞马/码垛规划/数据2/工件1/", //0 + "F:/ShangGu/项目/冠钦项目/宁波海瑞马/码垛规划/数据2/工件2/", //1 + "F:/ShangGu/项目/冠钦项目/宁波海瑞马/码垛规划/数据2/工件3/", //2 + }; + + SVzNLRange fileIdx[HRM_BIN_TEST_GROUP] = { + {1,7}, {1,9}, {1,7}, + }; + + const char* ver = wd_hybridPositioningVersion(); + printf("ver:%s 料筐尺寸计算\n", ver); + + for (int grp = 0; grp < HRM_BIN_TEST_GROUP; grp++) + { + SSG_planeCalibPara groundCalibPara; + //初始化成单位阵 + groundCalibPara.planeCalib[0] = 1.0; + groundCalibPara.planeCalib[1] = 0.0; + groundCalibPara.planeCalib[2] = 0.0; + groundCalibPara.planeCalib[3] = 0.0; + groundCalibPara.planeCalib[4] = 1.0; + groundCalibPara.planeCalib[5] = 0.0; + groundCalibPara.planeCalib[6] = 0.0; + groundCalibPara.planeCalib[7] = 0.0; + groundCalibPara.planeCalib[8] = 1.0; + groundCalibPara.planeHeight = -1.0; + for (int i = 0; i < 9; i++) + groundCalibPara.invRMatrix[i] = groundCalibPara.planeCalib[i]; + char calibFile[250]; + sprintf_s(calibFile, "%sground_calib_para.txt", dataPath[grp]); + groundCalibPara = _readCalibPara(calibFile); + + double binHeight = 640; //料筐最上边沿距地面高度 + for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) + { + //fidx = 15; + char _scan_file[256]; + sprintf_s(_scan_file, "%s隔板%d_LaserData_Jl26C177.txt", dataPath[grp], fidx); + std::vector> scanLines; + vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines); + _removeZeroLines_XYZ(scanLines); + if (scanLines.size() == 0) + continue; + + + long t1 = (long)GetTickCount64();//统计时间 + + int errCode = 0; +#if 0 + SSG_cornerParam cornerParam; + cornerParam.cornerTh = 60; //45度角 + cornerParam.scale = 10; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8; + cornerParam.minEndingGap = 10; // algoParam.bagParam.bagW / 4; + cornerParam.minEndingGap_z = 5.0; + cornerParam.jumpCornerTh_1 = 15; //水平角度，小于此角度视为水平 + cornerParam.jumpCornerTh_2 = 60; + //料筐码放：获取料筐尺寸、料筐姿态、料筐中心点坐标 + WD_HRM_BinInfo binInfo = wd_HRM_getBinSize( + scanLines, + cornerParam, + &errCode); +#else + //料筐码放：获取料筐尺寸、料筐姿态、料筐中心点坐标 + WD_HRM_BinInfo binInfo = wd_HRM_getBinSize( + scanLines, + groundCalibPara, + binHeight, //料筐高度 + &errCode); +#endif + long t2 = (long)GetTickCount64(); + printf("%s: %d(ms), errCode=%d ...", _scan_file, (int)(t2 - t1), errCode); + //输出测试结果 + sprintf_s(_scan_file, "%sresult/LaserLine%d_result.txt", dataPath[grp], fidx); + + _outputRGBDResult_XYZ_RGBD_binInfo(_scan_file, scanLines, binInfo); + sprintf_s(_scan_file, "%sresult/%d_bin_info.txt", dataPath[grp], fidx); + _outputBinInfo(_scan_file, binInfo); + + sprintf_s(calibFile, "%sresult/LaserLine%d_bin_projection.png", dataPath[grp], fidx); + double rpy[3] = { 0, 0, 0 }; // { -30, 15, 0 }; //{ 0,-45, 0 }; // + double angleDrawLen = 500; + double displayScale = 1.0; + cv::String imgName(calibFile); + _genXOYProjectionImage_binInfo(imgName, scanLines, displayScale, binInfo, rpy, angleDrawLen); + printf("done\n"); + } + } +} +//海瑞马工件尺寸测量 +void HaiRuiMa_workpieceSizeCompute_test(void) +{ + const char* dataPath[HRM_BIN_TEST_GROUP] = { + + "F:/ShangGu/项目/冠钦项目/宁波海瑞马/码垛规划/数据2/工件1/", //0 + "F:/ShangGu/项目/冠钦项目/宁波海瑞马/码垛规划/数据2/工件2/", //1 + "F:/ShangGu/项目/冠钦项目/宁波海瑞马/码垛规划/数据2/工件3/", //2 + }; + + SVzNLRange fileIdx[HRM_BIN_TEST_GROUP] = { + {1,7}, {1,9}, {1,7}, + }; + + const char* ver = wd_hybridPositioningVersion(); + printf("ver:%s 工件尺寸测量 \n", ver); + + for (int grp = 1; grp < HRM_BIN_TEST_GROUP; grp++) + { + SSG_planeCalibPara groundCalibPara; + //初始化成单位阵 + groundCalibPara.planeCalib[0] = 1.0; + groundCalibPara.planeCalib[1] = 0.0; + groundCalibPara.planeCalib[2] = 0.0; + groundCalibPara.planeCalib[3] = 0.0; + groundCalibPara.planeCalib[4] = 1.0; + groundCalibPara.planeCalib[5] = 0.0; + groundCalibPara.planeCalib[6] = 0.0; + groundCalibPara.planeCalib[7] = 0.0; + groundCalibPara.planeCalib[8] = 1.0; + groundCalibPara.planeHeight = -1.0; + for (int i = 0; i < 9; i++) + groundCalibPara.invRMatrix[i] = groundCalibPara.planeCalib[i]; + char calibFile[250]; + sprintf_s(calibFile, "%sground_calib_para.txt", dataPath[grp]); + groundCalibPara = _readCalibPara(calibFile); + + double binHeight = 640; //料筐最上边沿距地面高度 + for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) + { + //fidx = 15; + char _scan_file[256]; + sprintf_s(_scan_file, "%s工件%d_LaserData_Jl26C177.txt", dataPath[grp], fidx); + std::vector> scanLines; + vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines); + _removeZeroLines_XYZ(scanLines); + if (scanLines.size() == 0) + continue; + + + long t1 = (long)GetTickCount64();//统计时间 + + int errCode = 0; + SSG_cornerParam cornerParam; + cornerParam.cornerTh = 60; //45度角 + cornerParam.scale = 10; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8; + cornerParam.minEndingGap = 10; // algoParam.bagParam.bagW / 4; + cornerParam.minEndingGap_z = 5.0; + cornerParam.jumpCornerTh_1 = 15; //水平角度，小于此角度视为水平 + cornerParam.jumpCornerTh_2 = 60; + + //标准工件尺寸。目前3种工件尺寸 + std::vector standardWorkpieceSize; + standardWorkpieceSize.resize(3); + //工件1 + standardWorkpieceSize[0].center = { 0, 0, 0 }; + standardWorkpieceSize[0].layerZValue = 0; + standardWorkpieceSize[0].workpieceHeight = 40.0; + standardWorkpieceSize[0].workpieceRadius = 45.0; + //工件2 + standardWorkpieceSize[1].center = { 0, 0, 0 }; + standardWorkpieceSize[1].layerZValue = 0; + standardWorkpieceSize[1].workpieceHeight = 70.0; + standardWorkpieceSize[1].workpieceRadius = 45.0; + //工件3 + standardWorkpieceSize[1].center = { 0, 0, 0 }; + standardWorkpieceSize[1].layerZValue = 0; + standardWorkpieceSize[1].workpieceHeight = 60.0; + standardWorkpieceSize[1].workpieceRadius = 62.5; + + //料筐码放：获取工件尺寸 + WD_HRM_workpieceSizeInfo workpieceInfo = wd_HRM_getWorkpieceSize( + scanLines, + standardWorkpieceSize, + cornerParam, + groundCalibPara, + &errCode); + + long t2 = (long)GetTickCount64(); + printf("%s: %d(ms), errCode=%d ...", _scan_file, (int)(t2 - t1), errCode); + //输出测试结果 + sprintf_s(_scan_file, "%sresult/LaserLine%d_workpiece_result.txt", dataPath[grp], fidx); + + std::vector poseInfo; + WD_workpieceInfo a_pose; + memset(&a_pose, 0, sizeof(WD_workpieceInfo)); + a_pose.center = workpieceInfo.center; + a_pose.value = workpieceInfo.workpieceRadius; + a_pose.z_dir = { 0, 0, 1.0 }; + poseInfo.push_back(a_pose); + _outputRGBDResult_XYZ_RGBD_workpieceInfo(_scan_file, scanLines, poseInfo); + sprintf_s(_scan_file, "%sresult/%d_workpiece_info.txt", dataPath[grp], fidx); + _outputWorkpieceInfo(_scan_file, workpieceInfo); + + sprintf_s(calibFile, "%sresult/LaserLine%d_workpiece_projection.png", dataPath[grp], fidx); + double rpy[3] = { 0, 0, 0 }; // { -30, 15, 0 }; //{ 0,-45, 0 }; // + double angleDrawLen = 500; + double displayScale = 1.0; + cv::String imgName(calibFile); + _genXOYProjectionImage_workpieceInfo(imgName, scanLines, displayScale, poseInfo, rpy, angleDrawLen); + printf("done\n"); + } + } +} +//海瑞马码垛规划 +void HaiRuiMa_positionPlanning_test(void) +{ + const char* dataPath[HRM_BIN_TEST_GROUP] = { + + "F:/ShangGu/项目/冠钦项目/宁波海瑞马/码垛规划/数据2/工件1/", //0 + "F:/ShangGu/项目/冠钦项目/宁波海瑞马/码垛规划/数据2/工件2/", //1 + "F:/ShangGu/项目/冠钦项目/宁波海瑞马/码垛规划/数据2/工件3/", //2 + }; + + SVzNLRange fileIdx[HRM_BIN_TEST_GROUP] = { + {1,7}, {1,9}, {1,7}, + }; + + const char* ver = wd_hybridPositioningVersion(); + printf("ver:%s 码垛规划\n", ver); + + for (int grp = 0; grp < HRM_BIN_TEST_GROUP; grp++) + { + SSG_planeCalibPara groundCalibPara; + //初始化成单位阵 + groundCalibPara.planeCalib[0] = 1.0; + groundCalibPara.planeCalib[1] = 0.0; + groundCalibPara.planeCalib[2] = 0.0; + groundCalibPara.planeCalib[3] = 0.0; + groundCalibPara.planeCalib[4] = 1.0; + groundCalibPara.planeCalib[5] = 0.0; + groundCalibPara.planeCalib[6] = 0.0; + groundCalibPara.planeCalib[7] = 0.0; + groundCalibPara.planeCalib[8] = 1.0; + groundCalibPara.planeHeight = -1.0; + for (int i = 0; i < 9; i++) + groundCalibPara.invRMatrix[i] = groundCalibPara.planeCalib[i]; + char calibFile[250]; + sprintf_s(calibFile, "%sground_calib_para.txt", dataPath[grp]); + groundCalibPara = _readCalibPara(calibFile); + + double binHeight = 640; //料筐最上边沿距地面高度 + for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) + { + //fidx = 15; + char _scan_file[256]; + sprintf_s(_scan_file, "%s隔板%d_LaserData_Jl26C177.txt", dataPath[grp], fidx); + std::vector> scanLines; + vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines); + _removeZeroLines_XYZ(scanLines); + if (scanLines.size() == 0) + continue; + //地面调平 + for (int i = 0; i < (int)scanLines.size(); i++) + { + wd_lineDataR(scanLines[i], groundCalibPara.planeCalib, -1);//调平 + for (int j = 0; j < (int)scanLines[i].size(); j++) + scanLines[i][j].nPointIdx = 0; + } + + //读取料筐参数 + sprintf_s(_scan_file, "%sresult/%d_bin_info.txt", dataPath[grp], fidx); + WD_HRM_BinInfo binInfo = _readBinInfo(_scan_file); + //读取工件参数 + sprintf_s(_scan_file, "%sresult/%d_workpiece_info.txt", dataPath[grp], fidx); + WD_HRM_workpieceSizeInfo workpieceInfo = _readWorkpieceInfo(_scan_file); + + SSG_size2D realBoardSize; //实际的托板大小 + realBoardSize.width = 720.0; + realBoardSize.height = 520.0; + + double guardingInterval = 3.0; //工作与工件的保护间隔 + + long t1 = (long)GetTickCount64();//统计时间 + + int out_rows = 0, out_cols = 0, isLastLayere = 0; + std::vector< WD_workpieceInfo> planningPositions; + wd_HRM_PlanBinPlacement( + binInfo, + realBoardSize, + workpieceInfo, + groundCalibPara, + guardingInterval, //工作与工件的保护间隔 + planningPositions, + &out_rows, + &out_cols, + &isLastLayere); + + long t2 = (long)GetTickCount64(); + printf("%s: %d(ms), rows=%d cols=%d, isLastLayer=%d...", _scan_file, (int)(t2 - t1), out_rows, out_cols, isLastLayere); + //输出测试结果 + sprintf_s(_scan_file, "%sresult/%d_planning_result.txt", dataPath[grp], fidx); + _outputWorkpieceInfo(_scan_file, planningPositions); + + //旋转到调平坐标系中 + for (int i = 0; i < (int)planningPositions.size(); i++) + { + planningPositions[i].center = wd_ptRotate(planningPositions[i].center, groundCalibPara.planeCalib); + planningPositions[i].x_dir = wd_ptRotate(planningPositions[i].x_dir, groundCalibPara.planeCalib); + planningPositions[i].y_dir = wd_ptRotate(planningPositions[i].y_dir, groundCalibPara.planeCalib); + planningPositions[i].z_dir = wd_ptRotate(planningPositions[i].z_dir, groundCalibPara.planeCalib); + } + sprintf_s(_scan_file, "%sresult/LaserLine%d_planning_result.txt", dataPath[grp], fidx); + _outputRGBDResult_XYZ_RGBD_planningResult(_scan_file, scanLines, binInfo, planningPositions, workpieceInfo.workpieceHeight); + + sprintf_s(calibFile, "%sresult/LaserLine%d_planning_result.png", dataPath[grp], fidx); + double rpy[3] = { 0, 0, 0 }; // { -30, 15, 0 }; //{ 0,-45, 0 }; // + double angleDrawLen = 500; + double displayScale = 1.0; + cv::String imgName(calibFile); + _genXOYProjectionImage_planningResult(imgName, scanLines, displayScale, binInfo, planningPositions, rpy, angleDrawLen); + printf("done\n"); + } + } +} +typedef enum +{ + keSG_2D3D定位_海瑞马_地面调平 = 0, + keSG_2D3D定位_海瑞马_锥形工件, + keSG_2D3D定位_海瑞马_转子芯, + keSG_2D3D定位_海瑞马_码垛料筐定位, + keSG_2D3D定位_海瑞马_码垛工件尺寸测量, + keSG_2D3D定位_海瑞马_码垛位置规划, +} ESG_testMode; +int main() +{ + //ESG_testMode testMode = keSG_2D3D定位_海瑞马_地面调平; + //ESG_testMode testMode = keSG_2D3D定位_海瑞马_锥形工件; + //ESG_testMode testMode = keSG_2D3D定位_海瑞马_转子芯; + //ESG_testMode testMode = keSG_2D3D定位_海瑞马_码垛料筐定位; + //ESG_testMode testMode = keSG_2D3D定位_海瑞马_码垛工件尺寸测量; + ESG_testMode testMode = keSG_2D3D定位_海瑞马_码垛位置规划; + + if (keSG_2D3D定位_海瑞马_锥形工件 == testMode) + HaiRuiMa_TaperedWorkpiece_test(); + + else if (keSG_2D3D定位_海瑞马_地面调平 == testMode) + HaiRuiMa_groundCalib_test(); + else if (keSG_2D3D定位_海瑞马_码垛料筐定位 == testMode) + HaiRuiMa_BinSizeCompute_test(); + else if (keSG_2D3D定位_海瑞马_码垛工件尺寸测量 == testMode) + HaiRuiMa_workpieceSizeCompute_test(); + else if (keSG_2D3D定位_海瑞马_码垛位置规划 == testMode) + { + HaiRuiMa_BinSizeCompute_test(); + HaiRuiMa_workpieceSizeCompute_test(); + HaiRuiMa_positionPlanning_test(); + } + + +} diff --git a/hybridPosePositioning_test/hybridPosePositioning_test.vcxproj b/hybridPosePositioning_test/hybridPosePositioning_test.vcxproj new file mode 100644 index 0000000..0efff89 --- /dev/null +++ b/hybridPosePositioning_test/hybridPosePositioning_test.vcxproj @@ -0,0 +1,145 @@ + + + + + Debug + Win32 + + + Release + Win32 + + + Debug + x64 + + + Release + x64 + + + + + + + 17.0 + Win32Proj + {288e402c-183a-49ad-8ee1-5b1ae4e6f617} + hybridPosePositioningtest + 10.0 + + + + Application + true + v143 + Unicode + + + Application + false + v143 + true + Unicode + + + Application + true + v143 + Unicode + + + Application + false + v143 + true + Unicode + + + + + + + + + + + + + + + + + + + + + $(SolutionDir)build\$(Platform)\$(Configuration)\ + ..\..\thirdParty\VzNLSDK\Inc;..\sourceCode;..\sourceCode\inc;$(IncludePath) + + + $(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 + + + + + Level3 + true + _CRT_SECURE_NO_WARNINGS;_DEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + ..\..\thirdParty\opencv320\build\include; + + + Console + true + ..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Debug;%(AdditionalLibraryDirectories) + opencv_world320d.lib;baseAlgorithm.lib;hybridPosePositioning.lib;%(AdditionalDependencies) + + + + + Level3 + true + true + true + _CRT_SECURE_NO_WARNINGS;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + ..\..\thirdParty\opencv320\build\include; + + + Console + true + ..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Release;%(AdditionalLibraryDirectories) + opencv_world320.lib;baseAlgorithm.lib;hybridPosePositioning.lib;%(AdditionalDependencies) + + + + + + \ No newline at end of file diff --git a/sourceCode/hybridPosePositioning.cpp b/sourceCode/hybridPosePositioning.cpp new file mode 100644 index 0000000..9673378 --- /dev/null +++ b/sourceCode/hybridPosePositioning.cpp @@ -0,0 +1,1289 @@ +#include +#include "SG_baseDataType.h" +#include "SG_baseAlgo_Export.h" +#include "hybridPosePositioning_Export.h" +#include +#include + +#define _DEBUG_OUTPUT + +//version 1.0.0 : base version release to customer +//version 1.1.0 : 滮汾 +std::string m_strVersion = "HybridPositioning 1.1.0"; +const char* wd_hybridPositioningVersion(void) +{ + return m_strVersion.c_str(); +} + +//ˮƽװƽ +//ZƽеʱҪԵΪգˮƽ +//תΪƽƽ淨ΪֱIJ +SSG_planeCalibPara wd_getGroundCalibPara( + std::vector< std::vector>& scanLines) +{ + return sg_getPlaneCalibPara2(scanLines); +} + +//ˮƽʱ̬ƽȥ +void wd_lineDataR( + std::vector< SVzNL3DPosition>& a_line, + const double* camPoseR, + double groundH) +{ + lineDataRT_vector(a_line, camPoseR, groundH); +} + +SVzNL3DPosition _computeMinZPoint(std::vector& points) +{ + SVzNL3DPosition peak; + peak.nPointIdx = 0; + peak.pt3D = { 0.0, 0.0, 0.0 }; + for (int i = 0; i < (int)points.size(); i++) + { + if (points[i].pt3D.z < 1e-4) + continue; + + if (peak.pt3D.z < 1e-4) + peak = points[i]; + else if (peak.pt3D.z > points[i].pt3D.z) + peak = points[i]; + + } + return peak; +} + +SVzNL3DPoint _computeCentroid(std::vector& points) +{ + SVzNL3DPoint centroid = { 0.0, 0.0, 0.0 }; + int counter = 0; + for (int i = 0; i < (int)points.size(); i++) + { + if (points[i].pt3D.z < 1e-4) + continue; + + counter++; + centroid.x += points[i].pt3D.x; + centroid.y += points[i].pt3D.y; + centroid.z += points[i].pt3D.z; + } + centroid.x = centroid.x / counter; + centroid.y = centroid.y / counter; + centroid.z = centroid.z / counter; + return centroid; +} + +int _get2DRegion(SVzNLPositionD& a_pt2D3D, std::vector& objROIs) +{ + for (int i = 0; i < (int)objROIs.size(); i++) + { + if ((a_pt2D3D.ptLeft2D.x >= objROIs[i].roi.left) && (a_pt2D3D.ptLeft2D.x <= objROIs[i].roi.right) && + (a_pt2D3D.ptLeft2D.y >= objROIs[i].roi.top) && (a_pt2D3D.ptLeft2D.y <= objROIs[i].roi.bottom)) + return i; + } + return -1; +} + +bool _compareByZValue(SVzNL3DPosition& a, SVzNL3DPosition& b) +{ + return a.pt3D.z < b.pt3D.z; +} + +//Բɨ +void _genPolarScanData( + std::vector& points, + const double angleScale, + const SVzNL3DPoint polarCener, + std::vector>& polarScanData) +{ + int polarLines = (int)(360.0 / angleScale + 0.5); + polarScanData.resize(polarLines); + + int dataSize = (int)points.size(); + for (int i = 0; i < dataSize; i++) + { + int line = points[i].nPointIdx >> 16; + int ptIdx = points[i].nPointIdx & 0x0000FFFF; + SVzNL3DPoint& a_pt = points[i].pt3D; + + double angle = atan2(a_pt.y - polarCener.y, a_pt.x - polarCener.x); + angle = (angle / PI) * 180 + 180.0; + double R = sqrt(pow(a_pt.y - polarCener.y, 2) + pow(a_pt.x - polarCener.x, 2)); + + int angleLine = (int)(angle / angleScale + 0.5); + angleLine = angleLine % (int)polarScanData.size(); + + SWD_polarPt a_polarPt; + a_polarPt.lineIdx = line; + a_polarPt.ptIdx = ptIdx; + a_polarPt.x = a_pt.x; + a_polarPt.y = a_pt.y; + a_polarPt.z = a_pt.z; + a_polarPt.R = R; + a_polarPt.angle = angle; + polarScanData[angleLine].push_back(a_polarPt); + } +} + +//Բɨ, ͬԲṹ +void _genPolarScanData_2( + std::vector& points, + const double radiusScale, + const SVzNL3DPoint polarCener, + std::vector>& polarScanData) +{ + std::vector< SWD_polarPt> polarPoints; + int dataSize = (int)points.size(); + double rMax = 0; + for (int i = 0; i < dataSize; i++) + { + int line = points[i].nPointIdx >> 16; + int ptIdx = points[i].nPointIdx & 0x0000FFFF; + SVzNL3DPoint& a_pt = points[i].pt3D; + + double angle = atan2(a_pt.y - polarCener.y, a_pt.x - polarCener.x); + angle = (angle / PI) * 180 + 180.0; + double R = sqrt(pow(a_pt.y - polarCener.y, 2) + pow(a_pt.x - polarCener.x, 2)); + rMax = rMax < R ? R : rMax; + + SWD_polarPt a_polarPt; + a_polarPt.lineIdx = line; + a_polarPt.ptIdx = ptIdx; + a_polarPt.x = a_pt.x; + a_polarPt.y = a_pt.y; + a_polarPt.z = a_pt.z; + a_polarPt.R = R; + a_polarPt.angle = angle; + + polarPoints.push_back(a_polarPt); + } + + int circleNum = (int)(rMax / radiusScale) + 1; + polarScanData.resize(circleNum); + for (int i = 0; i < (int)polarPoints.size(); i++) + { + double r = polarPoints[i].R; + int idx = r / radiusScale; + polarScanData[idx].push_back(polarPoints[i]); + } +} + +bool compareByPolarScanR(const SWD_polarPt& a, const SWD_polarPt& b) { + return a.R < b.R; +} +bool compareByPolarScanAngle(const SWD_polarPt& a, const SWD_polarPt& b) { + return a.angle < b.angle; +} + +WD_workpieceInfo _computeWorkpiecePose(std::vector< std::vector>& scanLines) +{ + WD_workpieceInfo a_pose; + memset(&a_pose, 0, sizeof(WD_workpieceInfo)); + + int lineNum = (int)scanLines.size(); + int linePtNum = (int)scanLines[0].size(); + + std::vector> hLines; + hLines.resize(linePtNum); + for (int i = 0; i < linePtNum; i++) + hLines[i].resize(lineNum); + for (int line = 0; line < lineNum; line++) + { + for (int j = 0; j < linePtNum; j++) + { + scanLines[line][j].nPointIdx = 0; //ԭʼݵ0תʹã + hLines[j][line] = scanLines[line][j]; + hLines[j][line].pt3D.x = scanLines[line][j].pt3D.y; + hLines[j][line].pt3D.y = scanLines[line][j].pt3D.x; + } + } + //ֱˮƽɨ裬ˮƽʹֱǶ + + + + void wd_computeDirAngle_wholeLine2( + std::vector< SVzNL3DPosition>&line_data, + const double steppingScale, + const double invalidScale, //˳߶ȣǼЧ + std::vector< SSG_pntDirAngle>&ptDirAngles + ); + + //Ƿ͹ + return a_pose; +} + + +void wd_HRM_RotorCorePositioning( + std::vector< std::vector>& scanLinesInput, + std::vector& objROIs, + const SSG_planeCalibPara groundCalibPara, + std::vector< WD_workpieceInfo>& workpiecePositions, + int* errCode) +{ + *errCode = 0; + + if (objROIs.size() == 0) + { + *errCode = SX_ERR_ZERO_2D_OBJECTS; + return; + } + +#if 0 + for (int i = 0; i < lineNum; i++) + { //д + //ƽȥ + wd_lineDataR(scanLines[i], groundCalibPara.planeCalib, -1); + } +#endif + + int lineNum = (int)scanLinesInput.size(); + int linePtNum = (int)scanLinesInput[0].size(); + int maxU = 0; //2DͼCol + int maxV = 0; //2DͼRow + for (int line = 0; line < lineNum; line++) + { + for (int ptIdx = 0; ptIdx < (int)scanLinesInput[line].size(); ptIdx++) + { + if (scanLinesInput[line][ptIdx].pt3D.z < 1e-4) + continue; + + maxU = maxU < scanLinesInput[line][ptIdx].ptLeft2D.x ? scanLinesInput[line][ptIdx].ptLeft2D.x : maxU; + maxV = maxV < scanLinesInput[line][ptIdx].ptLeft2D.y ? scanLinesInput[line][ptIdx].ptLeft2D.y : maxV; + } + } + + //ͼ3dĶӦ + const int imgCols = maxU; + const int imgRows = maxV; + std::vector> mappingTable; //ͼС + mappingTable.resize(imgCols); //ɨ߷Ӧ + for (int i = 0; i < imgCols; i++) + mappingTable[i].resize(imgRows); + + for (int line = 0; line < lineNum; line++) + { + for (int ptIdx = 0; ptIdx < linePtNum; ptIdx++) + { + scanLinesInput[line][ptIdx].nPointIdx = 0; + if (scanLinesInput[line][ptIdx].pt3D.z < 1e-4) + continue; + + SWDIndexingVzPoint indexingPt; + indexingPt.lineIdx = line; + indexingPt.ptIdx = ptIdx; + indexingPt.point = scanLinesInput[line][ptIdx].pt3D; + int u = scanLinesInput[line][ptIdx].ptLeft2D.x; + int v = scanLinesInput[line][ptIdx].ptLeft2D.y; + mappingTable[u][v] = indexingPt; + } + } + + int objNum = (int)objROIs.size(); + for (int idx = 0; idx < objNum; idx++) + { + WD_objArea2D& obj_roi = objROIs[idx]; + int L = (int)(obj_roi.roi.left + 0.5); + int R = (int)(obj_roi.roi.right + 0.5); + int T = (int)(obj_roi.roi.top + 0.5); + int B = (int)(obj_roi.roi.bottom + 0.5); + + //ͳROIеɨߺPtIdxΧ + SVzNLRange roiLineIndice = { INT_MAX, 0 }; + SVzNLRange roiPtIndice = { INT_MAX, 0 }; + for (int x = L; x <= R; x++) + { + for (int y = T; y <= B; y++) + { + if (mappingTable[x][y].point.z > 1e-4) + { + int lineIdx = mappingTable[x][y].lineIdx; + int ptIdx = mappingTable[x][y].ptIdx; + scanLinesInput[lineIdx][ptIdx].nPointIdx = idx + 1; + + roiLineIndice.nMin = roiLineIndice.nMin > lineIdx ? lineIdx : roiLineIndice.nMin; + roiLineIndice.nMax = roiLineIndice.nMax < lineIdx ? lineIdx : roiLineIndice.nMax; + roiPtIndice.nMin = roiPtIndice.nMin > ptIdx ? ptIdx : roiPtIndice.nMin; + roiPtIndice.nMax = roiPtIndice.nMax < ptIdx ? ptIdx : roiPtIndice.nMax; + } + } + } + + //ROIеɨ + int roiLines = roiLineIndice.nMax - roiLineIndice.nMin + 1; + int roiLinePtNum = roiPtIndice.nMax - roiPtIndice.nMin + 1; + std::vector< std::vector> roiScanLines; + roiScanLines.resize(roiLines); + for (int line = 0; line < roiLines; line++) + roiScanLines[line].resize(roiLinePtNum); + for (int x = L; x <= R; x++) + { + for (int y = T; y <= B; y++) + { + if (mappingTable[x][y].point.z > 1e-4) + { + int lineIdx = mappingTable[x][y].lineIdx - roiLineIndice.nMin; + int ptIdx = mappingTable[x][y].ptIdx - roiPtIndice.nMin; + roiScanLines[lineIdx][ptIdx].pt3D = mappingTable[x][y].point; + } + } + } + + //жϹ࣬㹤̬ + + + } + return; + +} + +void wd_HRM_TaperedWorkpiecePositioning( + std::vector< std::vector>& scanLinesInput, + std::vector& objROIs, + const SSG_planeCalibPara groundCalibPara, + std::vector< WD_workpieceInfo>& workpiecePositions, + int* errCode) +{ + *errCode = 0; + + if (objROIs.size() == 0) + { + *errCode = SX_ERR_ZERO_2D_OBJECTS; + return; + } + + std::vector> rgnPoints; + rgnPoints.resize(objROIs.size()); + for (int line = 0; line < (int)scanLinesInput.size(); line++) + { + for (int ptIdx = 0; ptIdx < (int)scanLinesInput[line].size(); ptIdx++) + { + if (scanLinesInput[line][ptIdx].pt3D.z < 1e-4) + continue; + + int rgnIdx = _get2DRegion(scanLinesInput[line][ptIdx], objROIs); + if (rgnIdx >= 0) + { + SVzNL3DPosition a_rgnPt; + a_rgnPt.nPointIdx = (line << 16) | (ptIdx & 0xffff); + a_rgnPt.pt3D = scanLinesInput[line][ptIdx].pt3D; + rgnPoints[rgnIdx].push_back(a_rgnPt); + scanLinesInput[line][ptIdx].nPointIdx = rgnIdx + 1; + } + } + } + + //ͳÿregionߵ + std::vector< SVzNL3DPosition> rgnPeaks; + for (int i = 0; i < (int)rgnPoints.size(); i++) + { + SVzNL3DPosition peakPoint = _computeMinZPoint(rgnPoints[i]); + SVzNL3DPosition a_peak; + a_peak.nPointIdx = i; + a_peak.pt3D = peakPoint.pt3D; + rgnPeaks.push_back(a_peak); + } + + //߶ + std::sort(rgnPeaks.begin(), rgnPeaks.end(), _compareByZValue); + for (int i = 0; i < (int)rgnPeaks.size(); i++) + { + WD_workpieceInfo a_obj; + memset(&a_obj, 0, sizeof(WD_workpieceInfo)); + a_obj.center = rgnPeaks[i].pt3D; + workpiecePositions.push_back(a_obj); + } + return; + +} + +SSG_ROIRectD _getListROI(std::vector< SVzNL3DPosition>& listData) +{ + if (listData.size() == 0) + return { 0,0,0,0 }; + SSG_ROIRectD roi = { listData[0].pt3D.x, listData[0].pt3D.x, listData[0].pt3D.y, listData[0].pt3D.y }; + for (int i = 0; i < (int)listData.size(); i++) + { + roi.left = roi.left > listData[i].pt3D.x ? listData[i].pt3D.x : roi.left; + roi.right = roi.right < listData[i].pt3D.x ? listData[i].pt3D.x : roi.right; + roi.top = roi.top > listData[i].pt3D.y ? listData[i].pt3D.y : roi.top; + roi.bottom = roi.bottom < listData[i].pt3D.y ? listData[i].pt3D.y : roi.bottom; + } + return roi; +} + +double _getListMeanZ(std::vector< SVzNL3DPosition>& listData, SVzNLRangeD& zRange) +{ + if (listData.size() == 0) + return 0; + double meanZ = 0; + zRange.max = -1; + zRange.min = 0; + for (int i = 0; i < (int)listData.size(); i++) + { + meanZ += listData[i].pt3D.z; + if (zRange.max < 0) + { + zRange.max = listData[i].pt3D.z; + zRange.min = listData[i].pt3D.z; + } + else + { + zRange.max = zRange.max < listData[i].pt3D.z ? listData[i].pt3D.z : zRange.max; + zRange.min = zRange.min > listData[i].pt3D.z ? listData[i].pt3D.z : zRange.min; + } + } + meanZ = meanZ / (double)listData.size(); + return meanZ; +} + +//ʱתʱ > 0 ˳ʱתʱ < 0 +cv::Point2f _rotate2D(cv::Point2f pt, double sinTheta, double cosTheta) +{ + return (cv::Point2f((float)(pt.x * cosTheta - pt.y * sinTheta), (float)(pt.x * sinTheta + pt.y * cosTheta))); +} + +//ϿţȡϿߴ硢Ͽ̬Ͽĵ +#if 0 +//ԵСϿ +WD_HRM_BinInfo wd_HRM_getBinSize( + std::vector< std::vector>& scanLines, + const SSG_cornerParam cornerPara, + int* errCode) +{ + *errCode = 0; + WD_HRM_BinInfo resultPose; + memset(&resultPose, 0, sizeof(WD_HRM_BinInfo)); + + int lineNum = (int)scanLines.size(); + if (lineNum == 0) + { + *errCode = SG_ERR_3D_DATA_NULL; + return resultPose; + } + + int linePtNum = (int)scanLines[0].size(); + + //жݸʽǷΪgrid㷨ֻܴgridݸʽ + bool isGridData = true; + for (int line = 0; line < lineNum; line++) + { + if (linePtNum != (int)scanLines[line].size()) + { + isGridData = false; + break; + } + } + if (false == isGridData)//ݲʽ + { + *errCode = SG_ERR_NOT_GRID_FORMAT; + return resultPose; + } + + //ˮƽɨ + std::vector< std::vector> scanLines_h; + scanLines_h.resize(linePtNum); + for (int i = 0; i < linePtNum; i++) + scanLines_h[i].resize(lineNum); + for (int line = 0; line < lineNum; line++) + { + for (int j = 0; j < linePtNum; j++) + { + scanLines[line][j].nPointIdx = 0; //ԭʼݵ0תʹã + scanLines_h[j][line] = scanLines[line][j]; + scanLines_h[j][line].pt3D.x = scanLines[line][j].pt3D.y; + scanLines_h[j][line].pt3D.y = scanLines[line][j].pt3D.x; + } + } + for (int line = 0; line < linePtNum; line++) + { + for (int j = 0, j_max = (int)scanLines_h[line].size(); j < j_max; j++) + scanLines_h[line][j].nPointIdx = j; + } + + //㷨̣ + //1鴹ֱݲȥ + //2 + //3Ŀ + //4 + + //ڲ + SSG_cornerParam removeVertialPara = cornerPara; + removeVertialPara.scale = 3.0; + removeVertialPara.cornerTh = 60; + + std::vector> flags; + flags.resize(lineNum); + for (int i = 0; i < lineNum; i++) + { + flags[i].resize(linePtNum); + std::fill(flags[i].begin(), flags[i].end(), 0); + } + std::vector> zVertivalFlags; + for (int line = 0; line < lineNum; line++) + { + if (line == 700) + int kkk = 1; + std::vector line_verticalFlags; + wd_getXYVertialFeature_dirAngleMethod( + scanLines[line], + line, + removeVertialPara, + line_verticalFlags + ); + zVertivalFlags.push_back(line_verticalFlags); + + for (int i = 0; i < (int)line_verticalFlags.size(); i++) + { + if (line_verticalFlags[i] > 0) + flags[line][i] = 1; + } + } + + std::vector> zVertivalFlags_h; + for (int line = 0; line < linePtNum; line++) + { + if (line == 1177) + int kkk = 1; + std::vector line_verticalFlags; + wd_getXYVertialFeature_dirAngleMethod( + scanLines_h[line], + line, + removeVertialPara, + line_verticalFlags + ); + zVertivalFlags_h.push_back(line_verticalFlags); + + for (int i = 0; i < (int)line_verticalFlags.size(); i++) + { + if (line_verticalFlags[i] > 0) + flags[i][line] = 1; + } + } + for (int line = 0; line < lineNum; line++) + { + for (int j = 0; j < linePtNum; j++) + { + if (flags[line][j] > 0) + { + scanLines[line][j].pt3D.z = 0; + scanLines_h[j][line].pt3D.z = 0; + } + } + } + //һ + SSG_lineSegParam lineSegPara; + lineSegPara.distScale = 5.0; + lineSegPara.segGapTh_y = 5.0; + lineSegPara.segGapTh_z = 5.0; + const int minSegLen = 5; + for (int line = 0; line < lineNum; line++) + { + std::vector segs; + wd_getLineDataIntervals( + scanLines[line], + lineSegPara, + segs); + for (int i = 0; i < (int)segs.size(); i++) + { + if (segs[i].len <= minSegLen) + { + int idx0 = segs[i].start; + for (int j = 0; j < segs[i].len; j++) + flags[line][idx0 + j] = 1; + } + } + } + for (int line = 0; line < linePtNum; line++) + { + std::vector segs; + wd_getLineDataIntervals( + scanLines_h[line], + lineSegPara, + segs); + for (int i = 0; i < (int)segs.size(); i++) + { + if (segs[i].len <= minSegLen) + { + int idx0 = segs[i].start; + for (int j = 0; j < segs[i].len; j++) + flags[idx0 + j][line] = 1; + } + } + } + + //ע + for (int line = 0; line < lineNum; line++) + { + for (int j = 0; j < linePtNum; j++) + scanLines[line][j].nPointIdx = 0; //ԭʼݵ0תʹã + } + //ֱ߶ȥ + std::vector< SVzNL3DPosition> validPoints; + for (int line = 0; line < lineNum; line++) + { + for (int j = 0; j < linePtNum; j++) + { + if (flags[line][j] > 0) + scanLines[line][j].pt3D.z = 0; + + if (scanLines[line][j].pt3D.z > 1e-4) + { + SVzNL3DPosition a_vldPt; + a_vldPt.pt3D = scanLines[line][j].pt3D; + a_vldPt.nPointIdx = (line << 16) | (j & 0xffff); + validPoints.push_back(a_vldPt); + } + } + } + + // + //ڲ + //double minObjSize_w = 150; + //double minObjSize_h = 150; + + int clusterCheckWin = 5; + double clusterDist = 5.0; + int distType = 1; //0 - 2d distance; 1- 3d distance + std::vector> objClusters; //result + wd_pointClustering_speedUp( + validPoints, + lineNum, linePtNum, clusterCheckWin, // + clusterDist, + distType, + objClusters //result + ); + + //Ŀ + std::vector objMeanZ; + std::vector objZRange; + objMeanZ.resize(objClusters.size()); + objZRange.resize(objClusters.size()); + int maxSizeId = -1; + double maxSize = 0; + for (int i = 0; i < (int)objClusters.size(); i++) + { + SSG_ROIRectD a_roi = _getListROI(objClusters[i]); + double w = a_roi.right - a_roi.left; + double h = a_roi.bottom - a_roi.top; + double size = w * h; + + SVzNLRangeD zRange; + double meanZ = _getListMeanZ(objClusters[i], zRange); + objMeanZ[i] = meanZ; + objZRange[i] = zRange; + + if (maxSize < size) + { + maxSize = size; + maxSizeId = i; + } + + } + + //װͲıԵɨë + + + + std::vector< SVzNL3DPosition>& bottomCluster = objClusters[maxSizeId]; + //ע + //½flagsΪĿmask + for (int i = 0; i < lineNum; i++) + std::fill(flags[i].begin(), flags[i].end(), -1); + for (int i = 0; i < (int)bottomCluster.size(); i++) + { + int line = bottomCluster[i].nPointIdx >> 16; + int ptIdx = bottomCluster[i].nPointIdx & 0x0000FFFF; + scanLines[line][ptIdx].nPointIdx = 2; + flags[line][ptIdx] = i; //indexing + } + + //ʹPCA㷨 + SVzNL3DPoint vec_normal, vec_centroid; + + computePlaneNormalByPCA( + bottomCluster, + vec_normal, + vec_centroid); + + //ͶӰ + + + + + + if (vec_normal.z < 0) + vec_normal = { -vec_normal.x, -vec_normal.y, -vec_normal.z }; + resultPose.center = vec_centroid; + resultPose.bottomNormal = vec_normal; + + + return resultPose; +} +#else +//ϿԵĵΪ׼й滮 +WD_HRM_BinInfo wd_HRM_getBinSize( + std::vector< std::vector>& scanLines, + const SSG_planeCalibPara calibPara, + const double binHeight, //Ͽ߶ + int* errCode) +{ + *errCode = 0; + WD_HRM_BinInfo resultPose; + memset(&resultPose, 0, sizeof(WD_HRM_BinInfo)); + + //ڲ + SVzNLRangeD binTopSliceRange = {calibPara.planeHeight- binHeight-5.0, calibPara.planeHeight - binHeight + 5.0}; //Ͽ߶ZзΧ + + int lineNum = (int)scanLines.size(); + if (lineNum == 0) + { + *errCode = SG_ERR_3D_DATA_NULL; + return resultPose; + } + + int linePtNum = (int)scanLines[0].size(); + + //жݸʽǷΪgrid㷨ֻܴgridݸʽ + bool isGridData = true; + for (int line = 0; line < lineNum; line++) + { + if (linePtNum != (int)scanLines[line].size()) + { + isGridData = false; + break; + } + } + if (false == isGridData)//ݲʽ + { + *errCode = SG_ERR_NOT_GRID_FORMAT; + return resultPose; + } + + //ƽ + for (int i = 0; i < lineNum; i++) + wd_lineDataR(scanLines[i], calibPara.planeCalib, -1);//ƽ + + //Z + std::vector zSliceData; + for (int line = 0; line < lineNum; line++) + { + for (int j = 0; j < linePtNum; j++) + { + scanLines[line][j].nPointIdx = 0; + if ((scanLines[line][j].pt3D.z > binTopSliceRange.min) && (scanLines[line][j].pt3D.z < binTopSliceRange.max)) + { + SVzNL3DPosition a_pt; + a_pt.nPointIdx = (line << 16) | j & 0xffff; + a_pt.pt3D = scanLines[line][j].pt3D; + zSliceData.push_back(a_pt); + scanLines[line][j].nPointIdx = 1; //ע + } + } + } + + SVzNLRangeD dataZRange; + double zSliceZ = _getListMeanZ(zSliceData, dataZRange); + // + // СӾ + std::vector points; + for (int i = 0; i < (int)zSliceData.size(); i++) + { + cv::Point2f a_pt = cv::Point2f(zSliceData[i].pt3D.x, zSliceData[i].pt3D.y); + points.push_back(a_pt); + } + cv::RotatedRect rect = minAreaRect(points); + cv::Point2f vertices[4]; + rect.points(vertices); + double width = rect.size.width; //ͶӰĿ͸ + double height = rect.size.height; + if (width < height) + { + double tmp = height; + height = width; + width = tmp; + } + //̬vertices[0]ת + double dist_v0v3 = sqrt(pow(vertices[0].x - vertices[3].x, 2) + pow(vertices[0].y - vertices[3].y, 2)); + double width_diff = abs(dist_v0v3 - width); + double pose_yaw; + if (CV_VERSION == "3.2.0") + { + if (width_diff < 10.0)//width + { + pose_yaw = -rect.angle; + } + else //ȷ + { + pose_yaw = -rect.angle - 90; + if (pose_yaw < -90) + pose_yaw = 180 + pose_yaw; + } + } + else //if (CV_VERSION == "4.8.0") + { + if (width_diff < 10.0) //width + { + pose_yaw = -rect.angle; + } + else//ȷ + { + pose_yaw = -rect.angle + 90; + if (pose_yaw > 90) + pose_yaw = pose_yaw - 180; + } + } + //ϿϢ + double binZ = calibPara.planeHeight - binHeight; + double sinTheta = sin(-PI * pose_yaw / 180); + double cosTheta = cos(-PI * pose_yaw / 180); + resultPose.center = { rect.center.x, rect.center.y, binZ }; + resultPose.bottomNormal = { 0.0, 0.0, 1.0 }; + resultPose.x_dir = { cosTheta , sinTheta , 0.0 }; + resultPose.y_dir = vec3_cross(resultPose.bottomNormal, resultPose.x_dir); //˳y_dir + resultPose.length = width; + resultPose.width = height; + resultPose.binTopZ = zSliceZ; + resultPose.minRectVertex[0] = { vertices[0].x, vertices[0].y, binZ }; + resultPose.minRectVertex[1] = { vertices[1].x, vertices[1].y, binZ }; + resultPose.minRectVertex[2] = { vertices[2].x, vertices[2].y, binZ }; + resultPose.minRectVertex[3] = { vertices[3].x, vertices[3].y, binZ }; + + return resultPose; +} +#endif + +//Ͽţȡߴ +WD_HRM_workpieceSizeInfo wd_HRM_getWorkpieceSize( + std::vector< std::vector>& scanLines, + std::vector& standardWorkpieceSize, + const SSG_cornerParam cornerPara, + const SSG_planeCalibPara calibPara, + int* errCode) +{ + *errCode = 0; + WD_HRM_workpieceSizeInfo resultInfo; + memset(&resultInfo, 0, sizeof(WD_HRM_workpieceSizeInfo)); + + int lineNum = (int)scanLines.size(); + if (lineNum == 0) + { + *errCode = SG_ERR_3D_DATA_NULL; + return resultInfo; + } + + int linePtNum = (int)scanLines[0].size(); + + //жݸʽǷΪgrid㷨ֻܴgridݸʽ + bool isGridData = true; + for (int line = 0; line < lineNum; line++) + { + if (linePtNum != (int)scanLines[line].size()) + { + isGridData = false; + break; + } + } + if (false == isGridData)//ݲʽ + { + *errCode = SG_ERR_NOT_GRID_FORMAT; + return resultInfo; + } + + //ƽ + for (int i = 0; i < lineNum; i++) + wd_lineDataR(scanLines[i], calibPara.planeCalib, -1);//ƽ + + //ˮƽɨ + std::vector< std::vector> scanLines_h; + scanLines_h.resize(linePtNum); + for (int i = 0; i < linePtNum; i++) + scanLines_h[i].resize(lineNum); + for (int line = 0; line < lineNum; line++) + { + for (int j = 0; j < linePtNum; j++) + { + scanLines[line][j].nPointIdx = 0; //ԭʼݵ0תʹã + scanLines_h[j][line] = scanLines[line][j]; + scanLines_h[j][line].pt3D.x = scanLines[line][j].pt3D.y; + scanLines_h[j][line].pt3D.y = scanLines[line][j].pt3D.x; + } + } + for (int line = 0; line < linePtNum; line++) + { + for (int j = 0, j_max = (int)scanLines_h[line].size(); j < j_max; j++) + scanLines_h[line][j].nPointIdx = j; + } + + //㷨̣ + //1鴹ֱݲȥ + //2 + //3Ŀ + //4 + + //ڲ + SSG_cornerParam removeVertialPara = cornerPara; + removeVertialPara.scale = 3.0; + removeVertialPara.cornerTh = 60; + + std::vector> flags; + flags.resize(lineNum); + for (int i = 0; i < lineNum; i++) + { + flags[i].resize(linePtNum); + std::fill(flags[i].begin(), flags[i].end(), 0); + } + std::vector> zVertivalFlags; + for (int line = 0; line < lineNum; line++) + { + if (line == 700) + int kkk = 1; + std::vector line_verticalFlags; + wd_getXYVertialFeature_dirAngleMethod( + scanLines[line], + line, + removeVertialPara, + line_verticalFlags + ); + zVertivalFlags.push_back(line_verticalFlags); + + for (int i = 0; i < (int)line_verticalFlags.size(); i++) + { + if (line_verticalFlags[i] > 0) + flags[line][i] = 1; + } + } + + std::vector> zVertivalFlags_h; + for (int line = 0; line < linePtNum; line++) + { + if (line == 1177) + int kkk = 1; + std::vector line_verticalFlags; + wd_getXYVertialFeature_dirAngleMethod( + scanLines_h[line], + line, + removeVertialPara, + line_verticalFlags + ); + zVertivalFlags_h.push_back(line_verticalFlags); + + for (int i = 0; i < (int)line_verticalFlags.size(); i++) + { + if (line_verticalFlags[i] > 0) + flags[i][line] = 1; + } + } + for (int line = 0; line < lineNum; line++) + { + for (int j = 0; j < linePtNum; j++) + { + if (flags[line][j] > 0) + { + scanLines[line][j].pt3D.z = 0; + scanLines_h[j][line].pt3D.z = 0; + } + } + } + //һ + SSG_lineSegParam lineSegPara; + lineSegPara.distScale = 5.0; + lineSegPara.segGapTh_y = 5.0; + lineSegPara.segGapTh_z = 5.0; + const int minSegLen = 5; + for (int line = 0; line < lineNum; line++) + { + std::vector segs; + wd_getLineDataIntervals( + scanLines[line], + lineSegPara, + segs); + for (int i = 0; i < (int)segs.size(); i++) + { + if (segs[i].len <= minSegLen) + { + int idx0 = segs[i].start; + for (int j = 0; j < segs[i].len; j++) + flags[line][idx0 + j] = 1; + } + } + } + for (int line = 0; line < linePtNum; line++) + { + std::vector segs; + wd_getLineDataIntervals( + scanLines_h[line], + lineSegPara, + segs); + for (int i = 0; i < (int)segs.size(); i++) + { + if (segs[i].len <= minSegLen) + { + int idx0 = segs[i].start; + for (int j = 0; j < segs[i].len; j++) + flags[idx0 + j][line] = 1; + } + } + } + + //ע + for (int line = 0; line < lineNum; line++) + { + for (int j = 0; j < linePtNum; j++) + scanLines[line][j].nPointIdx = 0; //ԭʼݵ0תʹã + } + //ֱ߶ȥ + std::vector< SVzNL3DPosition> validPoints; + for (int line = 0; line < lineNum; line++) + { + for (int j = 0; j < linePtNum; j++) + { + if (flags[line][j] > 0) + scanLines[line][j].pt3D.z = 0; + + if (scanLines[line][j].pt3D.z > 1e-4) + { + SVzNL3DPosition a_vldPt; + a_vldPt.pt3D = scanLines[line][j].pt3D; + a_vldPt.nPointIdx = (line << 16) | (j & 0xffff); + validPoints.push_back(a_vldPt); + } + } + } + + // + //ڲ + //double minObjSize_w = 150; + //double minObjSize_h = 150; + + int clusterCheckWin = 5; + double clusterDist = 10.0; + int distType = 1; //0 - 2d distance; 1- 3d distance + std::vector> objClusters; //result + wd_pointClustering_speedUp( + validPoints, + lineNum, linePtNum, clusterCheckWin, // + clusterDist, + distType, + objClusters //result + ); + + //ȡĿΪ + std::vector objMeanZ; + std::vector objZRange; + std::vector< SSG_ROIRectD> objROIs; + objMeanZ.resize(objClusters.size()); + objZRange.resize(objClusters.size()); + objROIs.resize(objClusters.size()); + int maxSizeId = -1; + double maxSize = 0; + for (int i = 0; i < (int)objClusters.size(); i++) + { + SSG_ROIRectD a_roi = _getListROI(objClusters[i]); + objROIs[i] = a_roi; + + SVzNLRangeD zRange; + double meanZ = _getListMeanZ(objClusters[i], zRange); + objMeanZ[i] = meanZ; + objZRange[i] = zRange; + + double w = a_roi.right - a_roi.left; + double h = a_roi.bottom - a_roi.top; + double size = w * h; + if (maxSize < size) + { + maxSize = size; + maxSizeId = i; + } + } + + //ȡROIڵĿΪ + SSG_ROIRectD& layerBoardROI = objROIs[maxSizeId]; + double layerBoardZ = objMeanZ[maxSizeId]; + int workpieceClusterId = -1; + for (int i = 0; i< (int)objClusters.size(); i++) + { + if (i == maxSizeId) + continue; + + SSG_ROIRectD& a_roi = objROIs[i]; + double obj_z = objMeanZ[i]; + if ((a_roi.left > layerBoardROI.left) && (a_roi.right < layerBoardROI.right) && + (a_roi.top > layerBoardROI.top) && (a_roi.bottom < layerBoardROI.bottom) && (obj_z < layerBoardZ)) + { + if (workpieceClusterId < 0) + workpieceClusterId = i; + else if (objClusters[workpieceClusterId].size() < objClusters[i].size()) + workpieceClusterId = i; + } + } + + if(workpieceClusterId <0) + { + *errCode = SX_ERR_ZERO_OBJECTS; + return resultInfo; + } + + std::vector< SVzNL3DPosition>& layerBoardCluster = objClusters[maxSizeId]; + std::vector< SVzNL3DPosition>& workpieceCluster = objClusters[workpieceClusterId]; + double workpieceHeight = layerBoardZ - objZRange[workpieceClusterId].min; + //ע + for (int i = 0; i < (int)layerBoardCluster.size(); i++) + { + int line = layerBoardCluster[i].nPointIdx >> 16; + int ptIdx = layerBoardCluster[i].nPointIdx & 0x0000FFFF; + scanLines[line][ptIdx].nPointIdx = 1; + } + for (int i = 0; i < (int)workpieceCluster.size(); i++) + { + int line = workpieceCluster[i].nPointIdx >> 16; + int ptIdx = workpieceCluster[i].nPointIdx & 0x0000FFFF; + scanLines[line][ptIdx].nPointIdx = 2; + } + + //㹤СԲ + std::vector points_2d; + for (int i = 0; i < (int)workpieceCluster.size(); i++) + { + cv::Point2f a_pt = cv::Point2f(workpieceCluster[i].pt3D.x, workpieceCluster[i].pt3D.y); + points_2d.push_back(a_pt); + } + cv::Point2f center; + float r; + cv::minEnclosingCircle(points_2d, center, r); + + //׼ + double bestError = 0; + int bestId = -1; + for (int i = 0; i < (int)standardWorkpieceSize.size(); i++) + { + double err = abs(standardWorkpieceSize[i].workpieceHeight - workpieceHeight) + abs(standardWorkpieceSize[i].workpieceRadius - r); + if (bestId < 0) + { + bestId = i; + bestError = err; + } + else if (bestError > err) + { + bestId = i; + bestError = err; + } + } + //ͶӰ + resultInfo.center = { center.x, center.y, layerBoardZ }; + resultInfo.layerZValue = layerBoardZ; + resultInfo.workpieceHeight = standardWorkpieceSize[bestId].workpieceHeight; + resultInfo.workpieceRadius = standardWorkpieceSize[bestId].workpieceRadius; + return resultInfo; +} + +void planningFromCenter_1D(int num, double interval, double centerValue, std::vector& pos) +{ + pos.resize(num); + double halfInterval = interval / 2; + if (num % 2 == 1) // + { + int centerIdx = num / 2; + pos[centerIdx] = centerValue; + int j = 1; + for (int idx = centerIdx - 1; idx >= 0; idx--) + { + pos[idx] = centerValue - j * interval; + j++; + } + j = 1; + for (int idx = centerIdx + 1; idx < num; idx++) + { + pos[idx] = centerValue + j * interval; + j++; + } + } + else + { + int j = 0; + int halfSize = num / 2; + for (int idx = halfSize - 1; idx >= 0; idx--) + { + pos[idx] = centerValue - halfInterval - j * interval; + j++; + } + j = 0; + for (int idx = halfSize; idx < num; idx++) + { + pos[idx] = centerValue + halfInterval + j * interval; + j++; + } + } + return; +} + +//Ͽţλù滮 +void wd_HRM_PlanBinPlacement( + const WD_HRM_BinInfo binInfo, + const SSG_size2D realBoardSize, //ʵʵаС + const WD_HRM_workpieceSizeInfo workpieceInfo, + const SSG_planeCalibPara calibPara, + const double guardingInterval, //빤ı + std::vector< WD_workpieceInfo>& planningPositions, + int* out_rows, + int* out_cols, + int* isLastLayere) +{ + double diamter = workpieceInfo.workpieceRadius * 2 + guardingInterval; + double L = realBoardSize.width;// binInfo.length - guardingToSide * 2 + guardingInterval; + double W = realBoardSize.height; //binInfo.width - guardingToSide * 2 + guardingInterval; + + int cols = (int)(L / diamter); + int rows = (int)(W / diamter); + + //Ϊ׼λü + std::vector rowPos; + planningFromCenter_1D(rows, diamter, 0, rowPos); + std::vector colPos; + planningFromCenter_1D(cols, diamter, 0, colPos); + + int objNum = rows * cols; + planningPositions.resize(objNum); + for (int row = 0; row < rows; row++) + { + for(int col = 0; col < cols; col++) + { + WD_workpieceInfo a_pos; + a_pos.center = { colPos[col], rowPos[row], workpieceInfo.layerZValue }; + a_pos.value = workpieceInfo.workpieceRadius; + a_pos.workpieceType = 1; + a_pos.z_dir = { 0, 0, 1.0 }; + a_pos.y_dir = { 0, 0, 0 }; + a_pos.x_dir = { 0, 0, 0 }; + planningPositions[row * cols + col] = a_pos; + } + } + + //ת + double cosTheta = binInfo.x_dir.x; + double sinTheta = binInfo.x_dir.y; + for (int i = 0; i < (int)planningPositions.size(); i++) + { + cv::Point2f a_pt2D = cv::Point2f(planningPositions[i].center.x, planningPositions[i].center.y); + a_pt2D = _rotate2D(a_pt2D, sinTheta, cosTheta); + planningPositions[i].center.x = a_pt2D.x + binInfo.center.x; + planningPositions[i].center.y = a_pt2D.y + binInfo.center.y; + } + + double resiH = workpieceInfo.layerZValue - binInfo.binTopZ - workpieceInfo.workpieceHeight; + if (resiH < workpieceInfo.workpieceHeight) + *isLastLayere = 1; + else + *isLastLayere = 0; + *out_rows = rows; + *out_cols = cols; + + //תԭϵ + for (int i = 0; i < (int)planningPositions.size(); i++) + { + planningPositions[i].center = wd_ptRotate(planningPositions[i].center, calibPara.invRMatrix); + planningPositions[i].x_dir = wd_ptRotate(planningPositions[i].x_dir, calibPara.invRMatrix); + planningPositions[i].y_dir = wd_ptRotate(planningPositions[i].y_dir, calibPara.invRMatrix); + planningPositions[i].z_dir = wd_ptRotate(planningPositions[i].z_dir, calibPara.invRMatrix); + } + + return; +} \ No newline at end of file diff --git a/sourceCode/hybridPosePositioning_Export.h b/sourceCode/hybridPosePositioning_Export.h new file mode 100644 index 0000000..10f951e --- /dev/null +++ b/sourceCode/hybridPosePositioning_Export.h @@ -0,0 +1,101 @@ +#pragma once + +#include "SG_algo_Export.h" +#include + +#define _OUTPUT_DEBUG_DATA 1 + +typedef struct +{ + int workpieceType; + SSG_ROIRectD roi; + double score2D; //2DָŶ +}WD_objArea2D; + +typedef struct +{ + int workpieceType; + SVzNL3DPoint center; + SVzNL3DPoint z_dir; //zһ + SVzNL3DPoint y_dir; //yһ + SVzNL3DPoint x_dir; //xһ + double value; +}WD_workpieceInfo; + +//ϿϢ +typedef struct +{ + double length; + double width; + double binTopZ; //ϿߴZֵ + SVzNL3DPoint center; // + SVzNL3DPoint bottomNormal; //ķ + SVzNL3DPoint x_dir; //Xбǣ + SVzNL3DPoint y_dir; //Xбǣ + SVzNL3DPoint minRectVertex[4]; //СӾζ +}WD_HRM_BinInfo; + +typedef struct +{ + double workpieceRadius; //Բι뾶 + double workpieceHeight; //߶ + double layerZValue; // + SVzNL3DPoint center; // +}WD_HRM_workpieceSizeInfo; + +//汾 +SG_APISHARED_EXPORT const char* wd_hybridPositioningVersion(void); + +//ˮƽװƽ +//תΪƽƽ淨ΪֱIJ +SG_APISHARED_EXPORT SSG_planeCalibPara wd_getGroundCalibPara( + std::vector< std::vector>& scanLines); + +//ˮƽʱ̬ƽȥ +SG_APISHARED_EXPORT void wd_lineDataR( + std::vector< SVzNL3DPosition>& a_line, + const double* camPoseR, + double groundH); + +//2D ROI3Dλ +SG_APISHARED_EXPORT void wd_HRM_TaperedWorkpiecePositioning( + std::vector< std::vector>& scanLinesInput, + std::vector& objROIs, + const SSG_planeCalibPara groundCalibPara, + std::vector< WD_workpieceInfo>& workpiecePositions, + int* errCode); + +#if 0 +//ϿţȡϿߴ硢Ͽ̬Ͽĵ +SG_APISHARED_EXPORT WD_HRM_BinInfo wd_HRM_getBinSize( + std::vector< std::vector>& scanLines, + const SSG_cornerParam cornerPara, + int* errCode); +#else +//ϿԵĵΪ׼й滮 +SG_APISHARED_EXPORT WD_HRM_BinInfo wd_HRM_getBinSize( + std::vector< std::vector>& scanLines, + const SSG_planeCalibPara calibPara, + const double binHeight, //Ͽ߶ + int* errCode); +#endif + +//Ͽţȡߴ +SG_APISHARED_EXPORT WD_HRM_workpieceSizeInfo wd_HRM_getWorkpieceSize( + std::vector< std::vector>& scanLines, + std::vector& standardWorkpieceSize, + const SSG_cornerParam cornerPara, + const SSG_planeCalibPara calibPara, + int* errCode); + +//Ͽţλù滮 +SG_APISHARED_EXPORT void wd_HRM_PlanBinPlacement( + const WD_HRM_BinInfo binInfo, + const SSG_size2D realBoardSize, //ʵʵаС + const WD_HRM_workpieceSizeInfo workpieceInfo, + const SSG_planeCalibPara calibPara, + const double guardingInterval, //빤ı + std::vector< WD_workpieceInfo>& planningPositions, + int* out_rows, + int* out_cols, + int* isLastLayere); \ No newline at end of file