diff --git a/HC_planeLocalization/HC_planeLocalization.vcxproj b/HC_planeLocalization/HC_planeLocalization.vcxproj new file mode 100644 index 0000000..b6a4a28 --- /dev/null +++ b/HC_planeLocalization/HC_planeLocalization.vcxproj @@ -0,0 +1,158 @@ + + + + + Debug + Win32 + + + Release + Win32 + + + Debug + x64 + + + Release + x64 + + + + 17.0 + Win32Proj + {76989229-c695-43e6-af6a-ce8dd9980e16} + HCplaneLocalization + 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;..\..\thirdParty\opencv320\build\include;..\sourceCode;..\sourceCode\inc;$(IncludePath) + + + $(SolutionDir)build\$(Platform)\$(Configuration)\ + ..\..\thirdParty\VzNLSDK\Inc;..\..\thirdParty\opencv320\build\include;..\sourceCode;..\sourceCode\inc;$(IncludePath) + + + + Level3 + true + WIN32;_DEBUG;HCPLANELOCALIZATION_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) + true + Use + pch.h + + + Windows + true + false + + + + + Level3 + true + true + true + WIN32;NDEBUG;HCPLANELOCALIZATION_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) + true + Use + pch.h + + + Windows + true + false + + + + + Level3 + true + _CRT_SECURE_NO_WARNINGS;_DEBUG;HCPLANELOCALIZATION_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) + true + NotUsing + pch.h + + + 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;HCPLANELOCALIZATION_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) + true + NotUsing + pch.h + + + 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/HC_planeLocalization_test/HC_planeLocalization_test.cpp b/HC_planeLocalization_test/HC_planeLocalization_test.cpp new file mode 100644 index 0000000..d2a1918 --- /dev/null +++ b/HC_planeLocalization_test/HC_planeLocalization_test.cpp @@ -0,0 +1,650 @@ +// gasFillingPortPosition_test.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。 +// + +#include +#include +#include +#include +#include +#include "direct.h" +#include +#include "planeLocalization_Export.h" +#include +#include +#include + +typedef struct +{ + int r; + int g; + int b; +}SG_color; + +typedef struct +{ + int nPointIdx; + double x; + double y; + double z; + float r; + float g; + float b; +} SPointXYZRGB; + +SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, const 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; +} + +void wdReadLaserScanPointFromFile_XYZ_vector(const char* fileName, std::vector>& scanData) +{ + std::ifstream inputFile(fileName); + std::string linedata; + + if (inputFile.is_open() == false) + return; + + std::vector< SVzNL3DPosition> a_line; + int ptIdx = 0; + while (getline(inputFile, linedata)) + { + if (0 == strncmp("Line_", linedata.c_str(), 5)) + { + int ptSize = (int)a_line.size(); + if (ptSize > 0) + { + scanData.push_back(a_line); + } + a_line.clear(); + ptIdx = 0; + } + else if (0 == strncmp("{", linedata.c_str(), 1)) + { + float X, Y, Z; + int imageY = 0; + float leftX, leftY; + float rightX, rightY; + sscanf_s(linedata.c_str(), "{%f,%f,%f}-{%f,%f}-{%f,%f}", &X, &Y, &Z, &leftX, &leftY, &rightX, &rightY); + SVzNL3DPosition a_pt; + a_pt.pt3D.x = X; + a_pt.pt3D.y = Y; + a_pt.pt3D.z = Z; + a_pt.nPointIdx = ptIdx; + ptIdx++; + a_line.push_back(a_pt); + } + } + //last line + int ptSize = (int)a_line.size(); + if (ptSize > 0) + { + scanData.push_back(a_line); + a_line.clear(); + } + + inputFile.close(); + return; +} + +void wd_gridScan_GetROIData(std::vector>& scanData, SVzNLRangeD roi_y, std::vector>& roiData) +{ + int lineNum = (int)scanData.size(); + int linePtNum = (int)scanData[0].size(); + int globalPtStart = INT_MAX; + int globalPtEnd = 0; + int lineStart = INT_MAX; + int lineEnd = 0; + for (int line = 0; line < lineNum; line++) + { + std::vector< SVzNL3DPosition >& lineData = scanData[line]; + int ptSize = (int)lineData.size(); + int vldNum = 0; + int ptStart = INT_MAX; + int ptEnd = 0; + for (int i = 0; i < ptSize; i++) + { + if (lineData[i].pt3D.z > 1e-4) + { + if ((lineData[i].pt3D.y < roi_y.min) || (lineData[i].pt3D.y > roi_y.max)) + lineData[i].pt3D = { 0.0, 0.0, 0.0 }; + } + + if (lineData[i].pt3D.z > 1e-4) + { + if (ptStart > i) + ptStart = i; + ptEnd = i; + vldNum++; + } + } + if (vldNum > 0) + { + if (globalPtStart > ptStart) + globalPtStart = ptStart; + if (globalPtEnd < ptEnd) + globalPtEnd = ptEnd; + + if (lineStart > line) + lineStart = line; + lineEnd = line; + } + } + int vldLineNum = lineEnd - lineStart + 1; + int vldPtNum = globalPtEnd - globalPtStart + 1; + + roiData.resize(vldLineNum); + for (int line = 0; line < vldLineNum; line++) + { + roiData[line].resize(vldPtNum); + for (int i = 0; i < vldPtNum; i++) + roiData[line][i] = scanData[line + lineStart][i + globalPtStart]; + } + return; +} + +void _outputScanDataFile(char* fileName, std::vector>& scanData, + float lineV, int maxTimeStamp, int clockPerSecond) +{ + std::ofstream sw(fileName); + + int lineNum = (int)scanData.size(); + sw << "LineNum:" << lineNum << std::endl; + sw << "DataType: 0" << std::endl; + sw << "ScanSpeed:" << lineV << std::endl; + sw << "PointAdjust: 1" << std::endl; + sw << "MaxTimeStamp:" << maxTimeStamp << "_" << clockPerSecond << std::endl; + for (int line = 0; line < lineNum; line++) + { + int nPositionCnt = (int)scanData[line].size(); + sw << "Line_" << line << "_0_" << nPositionCnt << std::endl; + for (int i = 0; i < nPositionCnt; i++) + { + SVzNL3DPosition& pt3D = scanData[line][i]; + float x = (float)pt3D.pt3D.x; + float y = (float)pt3D.pt3D.y; + float z = (float)pt3D.pt3D.z; + sw << "{ " << x << "," << y << "," << z << " }-"; + sw << "{0,0}-{0,0}" << std::endl; + } + } + sw.close(); +} + +void _outputPlaneInfo(char* fileName, SSX_planeInfo planeInfo) +{ + std::ofstream sw(fileName); + + char dataStr[250]; + sprintf_s(dataStr, 250, "距离: %g, 方位: %g", planeInfo.distance, planeInfo.dirAngle_deg); + sw << dataStr << std::endl; + sw.close(); +} + +void _outputRGBDScan_RGBD( + char* fileName, + std::vector>& scanLines +) +{ + int lineNum = (int)scanLines.size(); + std::ofstream sw(fileName); + int realLines = lineNum; + + 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]; + int ptIdx = pt3D->nPointIdx & 0xFFFF; + if (ptIdx > 0) + { + rgb = objColor[ptIdx % 8]; + size = 3; + } + else //if (pt3D->nPointIdx == 0) + { + 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; + } + } + sw.close(); +} + +void _outputRGBDScan_superCluster( + char* fileName, + std::vector>& scanLines +) +{ + int lineNum = (int)scanLines.size(); + std::ofstream sw(fileName); + int realLines = lineNum; + + 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]; + int superIdx = pt3D->nPointIdx & 0xffff; + int flag = pt3D->nPointIdx >> 16; + if (flag == 1) //机鼻 + { + rgb = { 255, 0, 0 }; + size = 10; + } + else if(flag == 2) //机身 + { + rgb = { 255, 0, 0 }; + size = 5; + } + else if (flag == 4) //发动机 + { + rgb = { 255, 255, 0 }; + size = 5; + } + else if (superIdx > 0) + { + rgb = objColor[superIdx % 8]; + size = 3; + } + else //if (pt3D->nPointIdx == 0) + { + 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; + } + } + sw.close(); +} + + +void _outputScanDataFile_vector(char* fileName, std::vector>& scanLines, bool removeZeros, int* headNullLines) +{ + std::ofstream sw(fileName); + int lineNum = (int)scanLines.size(); + if (lineNum == 0) + return; + + sw << "LineNum:" << lineNum << std::endl; + sw << "DataType: 0" << std::endl; + sw << "ScanSpeed: 0" << std::endl; + sw << "PointAdjust: 1" << std::endl; + sw << "MaxTimeStamp: 0_0" << std::endl; + + int lineIdx = 0; + int null_lines = 0; + bool counterNull = true; + for (int line = 0; line < lineNum; line++) + { + int linePtNum = (int)scanLines[line].size(); + if (linePtNum == 0) + continue; + + if (true == removeZeros) + { + int vldPtNum = 0; + for (int i = 0; i < linePtNum; i++) + { + if (scanLines[line][i].pt3D.z > 1e-4) + vldPtNum++; + } + linePtNum = vldPtNum; + } + sw << "Line_" << lineIdx << "_0_" << linePtNum << std::endl; + lineIdx++; + bool isNull = true; + for (int i = 0; i < linePtNum; i++) + { + SVzNL3DPoint* pt3D = &scanLines[line][i].pt3D; + if ((pt3D->z > 1e-4) && (isNull == true)) + isNull = false; + if ((true == removeZeros) && (pt3D->z < 1e-4)) + continue; + float x = (float)pt3D->x; + float y = (float)pt3D->y; + float z = (float)pt3D->z; + sw << "{ " << x << "," << y << "," << z << " }-"; + sw << "{0,0}-{0,0}" << std::endl; + } + if (true == counterNull) + { + if (true == isNull) + null_lines++; + else + counterNull = false; + } + } + *headNullLines = null_lines; + sw.close(); +} + +void _outputCalibPara(char* fileName, SSG_planeCalibPara calibPara) +{ + std::ofstream sw(fileName); + char dataStr[250]; + //调平矩阵 + sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.planeCalib[0], calibPara.planeCalib[1], calibPara.planeCalib[2]); + sw << dataStr << std::endl; + sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.planeCalib[3], calibPara.planeCalib[4], calibPara.planeCalib[5]); + sw << dataStr << std::endl; + sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.planeCalib[6], calibPara.planeCalib[7], calibPara.planeCalib[8]); + sw << dataStr << std::endl; + //地面高度 + sprintf_s(dataStr, 250, "%g", calibPara.planeHeight); + sw << dataStr << std::endl; + //反向旋转矩阵 + sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.invRMatrix[0], calibPara.invRMatrix[1], calibPara.invRMatrix[2]); + sw << dataStr << std::endl; + sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.invRMatrix[3], calibPara.invRMatrix[4], calibPara.invRMatrix[5]); + sw << dataStr << std::endl; + sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.invRMatrix[6], calibPara.invRMatrix[7], calibPara.invRMatrix[8]); + sw << dataStr << std::endl; + + sw.close(); +} + +SSG_planeCalibPara _readCalibPara(char* fileName) +{ + //设置初始结果 + double initCalib[9] = { + 1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0 }; + SSG_planeCalibPara planePara; + for (int i = 0; i < 9; i++) + planePara.planeCalib[i] = initCalib[i]; + planePara.planeHeight = -1.0; + for (int i = 0; i < 9; i++) + planePara.invRMatrix[i] = initCalib[i]; + + std::ifstream inputFile(fileName); + std::string linedata; + + if (inputFile.is_open() == false) + return planePara; + + //调平矩阵 + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.planeCalib[0], &planePara.planeCalib[1], &planePara.planeCalib[2]); + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.planeCalib[3], &planePara.planeCalib[4], &planePara.planeCalib[5]); + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.planeCalib[6], &planePara.planeCalib[7], &planePara.planeCalib[8]); + //地面高度 + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), "%lf", &planePara.planeHeight); + //反向旋转矩阵 + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.invRMatrix[0], &planePara.invRMatrix[1], &planePara.invRMatrix[2]); + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.invRMatrix[3], &planePara.invRMatrix[4], &planePara.invRMatrix[5]); + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.invRMatrix[6], &planePara.invRMatrix[7], &planePara.invRMatrix[8]); + + inputFile.close(); + return planePara; +} + +SSX_planeParkingParam _readParkingPara(char* fileName) +{ + SSX_planeParkingParam parkingPara; + memset(&parkingPara, 0, sizeof(SSX_planeParkingParam)); + + std::ifstream inputFile(fileName); + std::string linedata; + + if (inputFile.is_open() == false) + return parkingPara; + + //调平矩阵 + float x, y, z; + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), "parkingPoint: { %f, %f, %f }", &x, &y, &z); + parkingPara.parkingPoint = { x, y, z }; + + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), "guideLinePoint: { %f, %f, %f }", &x, &y, &z); + parkingPara.guideLinePoint = { x, y, z }; + + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), "guidingRange: %f", &x); + parkingPara.guidingRange = x; + + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), "parkingRange: %f", &x); + parkingPara.parkingRange = x; + + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), "distFromNoseToWheel: %f", &x); + parkingPara.distFromNoseToWheel = x; + + inputFile.close(); + return parkingPara; +} + +#define TEST_COMPUTE_GROUND_PARA 0 +#define TEST_COMPUTE_POSITION 1 +#define TEST_GROUP 1 +int main() +{ + const char* dataPath[TEST_GROUP] = { + "F:/ShangGu/项目/水木宏创/停机位停靠引导/数据/20260613_144419-波音737/", //0 + }; + + SVzNLRange fileIdx[TEST_GROUP] = { + {1,43}, + }; + + const char* ver = wd_PlaneLocalizationVersion(); + printf("ver:%s\n", ver); + +#if TEST_COMPUTE_GROUND_PARA + int cvtGrp = 0; + char _calib_datafile[256]; + sprintf_s(_calib_datafile, "%sLaserData_1.txt", dataPath[cvtGrp]); + std::vector> scanData; + wdReadLaserScanPointFromFile_XYZ_vector(_calib_datafile, scanData); + + int lineNum = (int)scanData.size(); + if (scanData.size() > 0) + { + SSG_planeCalibPara calibPara = wd_getGroundCalibPara(scanData); + + char calibFile[250]; + sprintf_s(calibFile, "%sground_calib_para.txt", dataPath[cvtGrp]); + _outputCalibPara(calibFile, calibPara); + +#if 1 + for (int fidx = fileIdx[cvtGrp].nMin; fidx <= fileIdx[cvtGrp].nMax; fidx++) + { + //fidx =4; + char _scan_file[256]; + sprintf_s(_scan_file, "%sLaserData_%d.txt", dataPath[cvtGrp], fidx); + std::vector> scanLines; + wdReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines); + if (scanLines.size() == 0) + continue; + + double groundHOffset = 200; //去除地面参数 + lineNum = (int)scanLines.size(); + for (int line = 0; line < lineNum; line++) + { + int linePtNum = scanLines[line].size(); + for (int j = 0; j < linePtNum; j++) + { + SVzNL3DPoint a_pt; + a_pt = _ptRotate(scanLines[line][j].pt3D, calibPara.planeCalib); + //if (a_pt.y >= (calibPara.planeHeight - groundHOffset)) + // a_pt = { 0.0, 0.0, 0.0 }; + scanLines[line][j].pt3D = a_pt; + } + } + sprintf_s(_scan_file, "%sLaserData_%d_ground_calib.txt", dataPath[cvtGrp], fidx); + int headNullLines = 0; + _outputScanDataFile_vector(_scan_file, scanLines, false, &headNullLines); + } +#endif + printf("%s: calib done!\n", _calib_datafile); + } +#endif + +#if TEST_COMPUTE_POSITION + for (int grp = 0; grp < 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); + + sprintf_s(calibFile, "%sparking_para.txt", dataPath[grp]); + SSX_planeParkingParam parkingParam = _readParkingPara(calibFile); + + for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) + { + //fidx =18; + char _scan_file[256]; + sprintf_s(_scan_file, "%sLaserData_%d.txt", dataPath[grp], fidx); + + std::vector> scanLines; + wdReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines); + + //转成plyTxt格式 + //sprintf_s(_scan_file, "%s%d_ply_Hi229229.txt", dataPath[grp], fidx); + //wdSavePlyTxt(_scan_file, scanLines); + + long t1 = (long)GetTickCount64();//统计时间 + + SSG_treeGrowParam growParam; + growParam.maxLineSkipNum = 10; + growParam.yDeviation_max = 300.0; + growParam.maxSkipDistance = 300.0; + growParam.zDeviation_max = 300.0;// + growParam.minLTypeTreeLen = 500; //mm + growParam.minVTypeTreeLen = 500; //mm + + int errCode = 0; +#if _OUTPUT_DEBUG_DATA + std::vector< std::vector> debugData; +#endif + SSX_planeInfo planeInfo = wd_planeLocalization( + scanLines, + groundCalibPara, + parkingParam, + growParam, +#if _OUTPUT_DEBUG_DATA + debugData, +#endif + & errCode); + + long t2 = (long)GetTickCount64(); + printf("%s: %d(ms)!\n", _scan_file, (int)(t2 - t1)); + //输出测试结果 +#if _OUTPUT_DEBUG_DATA + sprintf_s(_scan_file, "%sresult\\%d_superCluster.txt", dataPath[grp], fidx); + _outputRGBDScan_superCluster(_scan_file, debugData); +#endif + sprintf_s(_scan_file, "%sresult\\%d_result.txt", dataPath[grp], fidx); + _outputRGBDScan_RGBD(_scan_file, scanLines); + sprintf_s(_scan_file, "%sresult\\%d_fillingPort_info.txt", dataPath[grp], fidx); + _outputPlaneInfo(_scan_file, planeInfo); + } + } +#endif +} + +// 运行程序: Ctrl + F5 或调试 >“开始执行(不调试)”菜单 +// 调试程序: F5 或调试 >“开始调试”菜单 + +// 入门使用技巧: +// 1. 使用解决方案资源管理器窗口添加/管理文件 +// 2. 使用团队资源管理器窗口连接到源代码管理 +// 3. 使用输出窗口查看生成输出和其他消息 +// 4. 使用错误列表窗口查看错误 +// 5. 转到“项目”>“添加新项”以创建新的代码文件，或转到“项目”>“添加现有项”以将现有代码文件添加到项目 +// 6. 将来，若要再次打开此项目，请转到“文件”>“打开”>“项目”并选择 .sln 文件 diff --git a/HC_planeLocalization_test/HC_planeLocalization_test.vcxproj b/HC_planeLocalization_test/HC_planeLocalization_test.vcxproj new file mode 100644 index 0000000..6614c28 --- /dev/null +++ b/HC_planeLocalization_test/HC_planeLocalization_test.vcxproj @@ -0,0 +1,145 @@ + + + + + Debug + Win32 + + + Release + Win32 + + + Debug + x64 + + + Release + x64 + + + + 17.0 + Win32Proj + {e59226fe-e1fb-4f3b-9a93-a4173be316b8} + HCplaneLocalizationtest + 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;HC_planeLocalization.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;HC_planeLocalization.lib;%(AdditionalDependencies) + + + + + + + + + \ No newline at end of file diff --git a/sourceCode/SG_errCode.h b/sourceCode/SG_errCode.h index b4b8fe1..1123220 100644 --- a/sourceCode/SG_errCode.h +++ b/sourceCode/SG_errCode.h @@ -52,3 +52,8 @@ //ԣ춨λ #define SX_ERR_NO_HIGHEST_ROD -2701 +//ɻͣ +#define SX_ERR_NO_PLANE_FOUND -2801 +#define SX_ERR_NOSEPOINT_FAIL -2802 +#define SX_ERR_ENDINE_FAIL -2803 + diff --git a/sourceCode/planeLocalization.cpp b/sourceCode/planeLocalization.cpp new file mode 100644 index 0000000..af7a858 --- /dev/null +++ b/sourceCode/planeLocalization.cpp @@ -0,0 +1,740 @@ +#include +#include "SG_baseDataType.h" +#include "SG_baseAlgo_Export.h" +#include "planeLocalization_Export.h" +#include +#include + +//version 1.0.0 : base version release to customer +std::string m_strVersion = " PlaneLocalization 1.0.0"; +const char* wd_PlaneLocalizationVersion(void) +{ + return m_strVersion.c_str(); +} + +//ʹRANSACƼ +SSG_planeCalibPara wd_getGroundCalibPara( + std::vector< std::vector>& scanLines) +{ + //óʼ + double initCalib[9] = { + 1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0 }; + SSG_planeCalibPara groundParam; + for (int i = 0; i < 9; i++) + { + groundParam.planeCalib[i] = initCalib[i]; + groundParam.invRMatrix[i] = initCalib[i]; + } + groundParam.planeHeight = -1.0; + + std::vector points; + for (int line = 0; line < (int)scanLines.size(); line++) + { + for (int j = 0, j_max = (int)scanLines[line].size(); j < j_max; j++) + { + if ( (scanLines[line][j].pt3D.z > 1e-4) && (scanLines[line][j].pt3D.z <200000)) //200ڵ + { + cv::Point3d a_pt = cv::Point3d(scanLines[line][j].pt3D.x, scanLines[line][j].pt3D.y, scanLines[line][j].pt3D.z); + points.push_back(a_pt); + } + } + } + + double distTh = 1.0; + std::vector in_inliers; + Plane groundPlane = ransacFitPlane(points, in_inliers, distTh); + if (in_inliers.size() < 100) + { + return groundParam; + } + + SVzNL3DPoint vec_1; + if (groundPlane.B > 0) + vec_1 = { groundPlane.A, groundPlane.B, groundPlane.C }; + else + vec_1 = { -groundPlane.A, -groundPlane.B, -groundPlane.C }; + + SVzNL3DPoint vec_z = { 0, 1.0, 0.0 }; + groundParam = wd_computeRTMatrix(vec_1, vec_z); + + //߶ + std::vector groundPoints; + for (int i = 0; i < (int)in_inliers.size(); i++) + { + cv::Point3d rPt; + rPt.x = in_inliers[i].x * groundParam.planeCalib[0] + in_inliers[i].y * groundParam.planeCalib[1] + in_inliers[i].z * groundParam.planeCalib[2]; + rPt.y = in_inliers[i].x * groundParam.planeCalib[3] + in_inliers[i].y * groundParam.planeCalib[4] + in_inliers[i].z * groundParam.planeCalib[5]; + rPt.z = in_inliers[i].x * groundParam.planeCalib[6] + in_inliers[i].y * groundParam.planeCalib[7] + in_inliers[i].z * groundParam.planeCalib[8]; + groundPoints.push_back(rPt); + } + double groundY = 0; + for (int i = 0; i < (int)groundPoints.size(); i++) + groundY += groundPoints[i].y; + groundY = groundY / (int)groundPoints.size(); + groundParam.planeHeight = groundY; + return groundParam; +} + +int _checkAdjacency( + std::vector< SVzNL3DPosition>& cluster_1, + const int cluster1_idx, const int cluster2_idx, + std::vector>& clusterMask) +{ + int lineNum = (int)clusterMask.size(); + int linePtNum = (int)clusterMask[0].size(); + int cluster1_size = (int)cluster_1.size(); + + int adjacentNum = 0; + for (int i = 0; i < cluster1_size; i++) + { + SVzNL3DPosition& a_pt = cluster_1[i]; + // + int lineIdx = a_pt.nPointIdx >> 16; + int ptIdx = a_pt.nPointIdx & 0xFFFF; + if (clusterMask[lineIdx][ptIdx] != cluster1_idx) + continue; + + int leftCol = lineIdx - 1; + int rightCol = lineIdx + 1; + int topRow = ptIdx - 1; + int btmRow = ptIdx + 1; + if ((leftCol < 0) || (rightCol >= lineNum) || (topRow < 0) || (btmRow >= linePtNum)) + continue; + + if (clusterMask[leftCol][ptIdx] == cluster2_idx) + adjacentNum++; + if (clusterMask[rightCol][ptIdx] == cluster2_idx) + adjacentNum++; + if (clusterMask[lineIdx][topRow] == cluster2_idx) + adjacentNum++; + if (clusterMask[lineIdx][btmRow] == cluster2_idx) + adjacentNum++; + } + return adjacentNum; +} + +void _searchSuperCluster( + std::vector& a_superCluster, + std::vector>& adjacencyScoreTable, + std::vector& clusterFlags, + std::vector& clusterROIs, + const int adjacencyScoreTh, + const double adjacencyZDistTh) +{ + int clusterNum = adjacencyScoreTable.size(); + + int searchIdx = 0; + int seedIdx = a_superCluster[0]; + clusterFlags[seedIdx] = 1; + while (searchIdx < a_superCluster.size()) + { + int clusterIdx = a_superCluster[searchIdx]; + SVzNL3DRangeD& seedROI = clusterROIs[clusterIdx]; + for (int i = 0; i < clusterNum; i++) + { + if ((clusterFlags[i] > 0) || (i == seedIdx)) + continue; + + int score = adjacencyScoreTable[clusterIdx][i]; + if (score > adjacencyScoreTh) + { + a_superCluster.push_back(i); + clusterFlags[i] = 1; + } + else if (score > 0) + { + //:ʹZ + SVzNL3DRangeD& chkROI = clusterROIs[i]; + double zDist; + if (seedROI.zRange.min > chkROI.zRange.max) + zDist = seedROI.zRange.min - chkROI.zRange.max; + else if (chkROI.zRange.min > seedROI.zRange.max) + zDist = chkROI.zRange.min - seedROI.zRange.max; + else + zDist = 0; //ص + + if (zDist < adjacencyZDistTh) + { + a_superCluster.push_back(i); + clusterFlags[i] = 1; + } + } + } + searchIdx++; + } +} + +void _updateRoi3D(SVzNL3DRangeD& roi, SVzNL3DPoint& a_pt) +{ + if (a_pt.z > 1E-4) + { + if (roi.zRange.max < 0) + { + roi.xRange.min = a_pt.x; + roi.xRange.max = a_pt.x; + roi.yRange.min = a_pt.y; + roi.yRange.max = a_pt.y; + roi.zRange.min = a_pt.z; + roi.zRange.max = a_pt.z; + } + else + { + if (roi.xRange.min > a_pt.x) + roi.xRange.min = a_pt.x; + if (roi.xRange.max < a_pt.x) + roi.xRange.max = a_pt.x; + if (roi.yRange.min > a_pt.y) + roi.yRange.min = a_pt.y; + if (roi.yRange.max < a_pt.y) + roi.yRange.max = a_pt.y; + if (roi.zRange.min > a_pt.z) + roi.zRange.min = a_pt.z; + if (roi.zRange.max < a_pt.z) + roi.zRange.max = a_pt.z; + } + } + return; +} + +SVzNL3DRangeD _getSupcluster(std::vector& superClusterIndice, std::vector& objClustersROIs) +{ + int idx_0 = superClusterIndice[0]; + SVzNL3DRangeD mergeROI = objClustersROIs[idx_0]; + for (int i = 1; i < (int)superClusterIndice.size(); i++) + { + int idx = superClusterIndice[i]; + SVzNL3DRangeD& a_roi = objClustersROIs[idx]; + mergeROI.xRange.min = mergeROI.xRange.min > a_roi.xRange.min ? a_roi.xRange.min : mergeROI.xRange.min; + mergeROI.xRange.max = mergeROI.xRange.max < a_roi.xRange.max ? a_roi.xRange.max : mergeROI.xRange.max; + mergeROI.yRange.min = mergeROI.yRange.min > a_roi.yRange.min ? a_roi.yRange.min : mergeROI.yRange.min; + mergeROI.yRange.max = mergeROI.yRange.max < a_roi.yRange.max ? a_roi.yRange.max : mergeROI.yRange.max; + mergeROI.zRange.min = mergeROI.zRange.min > a_roi.zRange.min ? a_roi.zRange.min : mergeROI.zRange.min; + mergeROI.zRange.max = mergeROI.zRange.max < a_roi.zRange.max ? a_roi.zRange.max : mergeROI.zRange.max; + } + return mergeROI; +} +SSX_planeInfo wd_planeLocalization( + std::vector< std::vector>& scanLines, + const SSG_planeCalibPara groundCalibParam, + const SSX_planeParkingParam parkingParam, + const SSG_treeGrowParam growParam, +#if _OUTPUT_DEBUG_DATA + std::vector< std::vector>& debugData, +#endif + int* errCode) +{ + *errCode = 0; + //ڲ + double planeMinHeight = 4000; //С߶4 + double planeMinWidth = 25000; //С2.5 + double nearFarTh = 80000.0; //Զֽ + double groundHOffset = 200; //ȥ + double bodyHeadJudge_lenTh = 20000.0; //ͷжϵijޡ20 + SVzNLRangeD bodyRangeToNose = { -2000.0, 1000.0 }; //ԻǸ߶Ϊ׼ĸ߶ȷΧ + SVzNLRangeD sameRBodyRange = { 5000.0, 28000.0 }; //ԲͰΣԾǾΪ׼ + SVzNLRangeD engineToNoseDistRange = { 12000.0, 16000.0 }; + SVzNLRangeD bodyYRange = { groundCalibParam.planeHeight - 4500.0, groundCalibParam.planeHeight - 1500 }; //YΧ + + SSX_planeInfo planePoseInfo; + memset(&planePoseInfo, 0, sizeof(SSX_planeInfo)); + int lineNum = (int)scanLines.size(); + if (lineNum == 0) + { + *errCode = SG_ERR_3D_DATA_NULL; + return planePoseInfo; + } + + 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 planePoseInfo; + } + + //ָY + SVzNL3DPoint guideLineVec = { parkingParam.guideLinePoint.x - parkingParam.parkingPoint.x , 0, parkingParam.guideLinePoint.z - parkingParam.parkingPoint.z}; + if (guideLineVec.z < 0) + guideLineVec = {-guideLineVec.x, -guideLineVec.y, -guideLineVec.z}; + guideLineVec = vec3_normalize(guideLineVec); + SVzNL3DPoint targetVec = { 0, 0, 1 }; + SSG_planeCalibPara guideLineRotatePara = wd_computeRTMatrix(guideLineVec, targetVec); + + // + SVzNL3DPoint rotateParkingPoint = wd_ptRotate(parkingParam.parkingPoint, guideLineRotatePara.planeCalib); + SVzNL3DPoint chkPoint = wd_ptRotate(parkingParam.guideLinePoint, guideLineRotatePara.planeCalib); + //Ŀ귶Χ + SVzNLRangeD ROI_x_near = { rotateParkingPoint.x - parkingParam.parkingRange, rotateParkingPoint.x + parkingParam.parkingRange }; + SVzNLRangeD ROI_x_far = { rotateParkingPoint.x - parkingParam.parkingRange * 3, rotateParkingPoint.x + parkingParam.parkingRange* 3 }; //100m⣬ҪſΪɻڽ + SVzNLRangeD ROI_z = { rotateParkingPoint.z - parkingParam.distFromNoseToWheel - 2000.0, rotateParkingPoint.z + parkingParam.guidingRange }; + //ת ROI + + std::vector< std::vector> rotateROIData; + rotateROIData.resize(lineNum); + for (int line = 0; line < lineNum; line++) + { + rotateROIData[line].resize(linePtNum); + for (int j = 0; j < linePtNum; j++) + { + SVzNL3DPosition a_pt; + a_pt.nPointIdx = 0; + a_pt.pt3D = wd_ptRotate(scanLines[line][j].pt3D, groundCalibParam.planeCalib); + if (a_pt.pt3D.y >= (groundCalibParam.planeHeight - groundHOffset)) + a_pt.pt3D = { 0.0, 0.0, 0.0 }; + + //ٴתָY + a_pt.pt3D = wd_ptRotate(a_pt.pt3D, guideLineRotatePara.planeCalib); + rotateROIData[line][j] = a_pt; + } + } + + std::vector< std::vector> polarScanData; //ɨݣɨˮƽǡֱǺ;, + polarScanData.resize(lineNum); + std::vector validPoints; + for (int line = 0; line < lineNum; line++) + { + polarScanData[line].resize(rotateROIData[line].size()); + for (int j = 0, j_max = (int)rotateROIData[line].size(); j < j_max; j++) + { + if (rotateROIData[line][j].pt3D.z > 1e-4) //ȥ + { + SVzNL3DPosition a_pt; + a_pt.nPointIdx = (line << 16) | (j & 0xffff); + a_pt.pt3D = rotateROIData[line][j].pt3D; + validPoints.push_back(a_pt); + + //ɨǶȣʹתǰ + a_pt = scanLines[line][j]; + SWD_poloarScan2D a_polarData; + a_polarData.range_mm = sqrt(pow(a_pt.pt3D.x, 2) + pow(a_pt.pt3D.y, 2) + pow(a_pt.pt3D.z, 2)); + //ˮƽǣ + double yaw_rad = atan2(-a_pt.pt3D.x, a_pt.pt3D.z); + a_polarData.yaw_deg = yaw_rad * 180 / PI; + //ֱǣ + double pitch_rad = asin(-a_pt.pt3D.y / a_polarData.range_mm); + a_polarData.pitch_deg = pitch_rad * 180 / PI; + polarScanData[line][j] = a_polarData; + } + else + { + polarScanData[line][j].range_mm = 0; + polarScanData[line][j].yaw_deg = 0; + polarScanData[line][j].pitch_deg = 0; + } + scanLines[line][j].nPointIdx = 0; + } + } + +#if _OUTPUT_DEBUG_DATA + //ͶӰ + debugData.clear(); + debugData.resize(scanLines.size()); + for (int line = 0; line < lineNum; line++) + { + debugData[line].resize(scanLines[line].size()); + for (int j = 0, j_max = (int)scanLines[line].size(); j < j_max; j++) + { + debugData[line][j].nPointIdx = 0; + debugData[line][j].pt3D = rotateROIData[line][j].pt3D; + } + } +#endif + + // + //ʹSVzNL3DPositionnPointIdxʾ2DϢ16λLine 16λptIdx + //ʱԼ + int clusterCheckWin = 5; + double clusterDist = sqrt(pow(growParam.zDeviation_max, 2) + pow(growParam.maxSkipDistance, 2) + pow(growParam.yDeviation_max, 2)); + int distType = 1; //0 - 2d distance; 1- 3d distance + std::vector> allClusters; + wd_pointClustering_speedUp( + validPoints, + lineNum, linePtNum, clusterCheckWin, // + clusterDist, + distType, //0 - 2d distance; 1- 3d distance + allClusters //result + ); + + //ͳclusterROI + std::vector allClusterROIs; + allClusterROIs.resize(allClusters.size()); + for (int m = 0; m < (int)allClusters.size(); m++) + { + SVzNL3DRangeD a_roi3D; + memset(&a_roi3D, 0, sizeof(SVzNL3DRangeD)); + a_roi3D.zRange.max = -1; + for (int i = 0; i < (int)allClusters[m].size(); i++) + _updateRoi3D(a_roi3D, allClusters[m][i].pt3D); + allClusterROIs[m] = a_roi3D; + } + + //ROI + std::vector> objClusters; + std::vector objClustersROIs; + for (int m = 0; m < (int)allClusters.size(); m++) + { + SVzNL3DRangeD& a_roi = allClusterROIs[m]; + if ((a_roi.zRange.min - rotateParkingPoint.z) < nearFarTh) + { + if ((a_roi.xRange.min >= ROI_x_near.min) && (a_roi.xRange.max <= ROI_x_near.max) && + (a_roi.zRange.min >= ROI_z.min) && (a_roi.zRange.max <= ROI_z.max)) + { + objClusters.push_back(allClusters[m]); + objClustersROIs.push_back(a_roi); + } + } + else + { + if ((a_roi.xRange.min >= ROI_x_far.min) && (a_roi.xRange.max <= ROI_x_far.max) && + (a_roi.zRange.min >= ROI_z.min) && (a_roi.zRange.max <= ROI_z.max)) + { + objClusters.push_back(allClusters[m]); + objClustersROIs.push_back(a_roi); + } + } + } + + //֮Ĺϵ + //Mask + int clusterNum = (int)objClusters.size(); + std::vector> clusterMask; + clusterMask.resize(lineNum); + for (int i = 0; i < lineNum; i++) + { + clusterMask[i].resize(linePtNum); + std::fill(clusterMask[i].begin(), clusterMask[i].end(), -1); + } + for (int i = 0; i < clusterNum; i++) + { + std::vector< SVzNL3DPosition>& a_cluster = objClusters[i]; + int ptSize = (int)a_cluster.size(); + for (int j = 0; j < ptSize; j++) + { + SVzNL3DPosition& a_pt = a_cluster[j]; + int lineIdx = a_pt.nPointIdx >> 16; + int ptIdx = a_pt.nPointIdx & 0xffff; + clusterMask[lineIdx][ptIdx] = i; + } + } + //ڹϵ + std::vector> adjacencyScoreTable; + adjacencyScoreTable.resize(clusterNum); + for (int i = 0; i < clusterNum; i++) + adjacencyScoreTable[i].resize(clusterNum); + + for (int i = 0; i < clusterNum; i++) + { + std::vector< SVzNL3DPosition>& cluster_1 = objClusters[i]; + for (int j = i + 1; j < clusterNum; j++) + { + int score = _checkAdjacency(cluster_1, i, j, clusterMask); + adjacencyScoreTable[i][j] = score; + adjacencyScoreTable[j][i] = score; + } + } + + //ξɳ + int adjacencyScoreTh = 10; + double adjacencyZDistTh = 18000.0;//18׳ + std::vector> superClusers; //࣬ʹþID + std::vector clusterFlags; + clusterFlags.resize(objClusters.size()); + std::fill(clusterFlags.begin(), clusterFlags.end(), -1); + for (int i = 0; i < (int)objClusters.size(); i++) + { + if (clusterFlags[i] >= 0) + continue; + + std::vector a_superCluster; + a_superCluster.push_back(i); + //ݹ + _searchSuperCluster(a_superCluster, adjacencyScoreTable, clusterFlags, objClustersROIs, adjacencyScoreTh, adjacencyZDistTh); + + //ROI + SVzNL3DRangeD superROI = _getSupcluster(a_superCluster, objClustersROIs); + double h = abs(superROI.yRange.min - groundCalibParam.planeHeight); + double w = superROI.xRange.max - superROI.xRange.min; + double len = superROI.zRange.max - superROI.zRange.min; + + if( (h > planeMinHeight) && (w > planeMinWidth)) + superClusers.push_back(a_superCluster); + } + +#if _OUTPUT_DEBUG_DATA + //ע + for (int i = 0; i < (int)superClusers.size(); i++) + { + std::vector& a_superCluster = superClusers[i]; + int memberSize = (int)a_superCluster.size(); + for (int m = 0; m < memberSize; m++) + { + int clusterIdx = a_superCluster[m]; + std::vector< SVzNL3DPosition>& a_cluster = objClusters[clusterIdx]; + int ptSize = (int)a_cluster.size(); + for (int j = 0; j < ptSize; j++) + { + SVzNL3DPosition& a_pt = a_cluster[j]; + int lineIdx = a_pt.nPointIdx >> 16; + int ptIdx = a_pt.nPointIdx & 0xffff; + debugData[lineIdx][ptIdx].nPointIdx += (i + 1); + } + } + } +#endif + + if (superClusers.size() == 0) + { + *errCode = SX_ERR_NO_PLANE_FOUND; + return planePoseInfo; + } + + //ѡijΪɻ + std::vector superCluserPointSizes; + superCluserPointSizes.resize(superClusers.size()); + for (int i = 0; i < (int)superClusers.size(); i++) + { + std::vector& a_superCluster = superClusers[i]; + int superSize = 0; + int memberSize = (int)a_superCluster.size(); + for (int m = 0; m < memberSize; m++) + { + int clusterIdx = a_superCluster[m]; + std::vector< SVzNL3DPosition>& a_cluster = objClusters[clusterIdx]; + superSize += (int)a_cluster.size(); + } + superCluserPointSizes[i] = superSize; + } + + int bestId = 0; + for (int i = 1; i < (int)superCluserPointSizes.size(); i++) + { + if (superCluserPointSizes[bestId] < superCluserPointSizes[i]) + bestId = i; + } + + std::vector& planeSuperCluster = superClusers[bestId]; + int planeClusterSize = (int)planeSuperCluster.size(); + //Ѱһͷλ: ROIǰZС + int noseClusterId = -1; + for (int i = 0; i < planeClusterSize; i++) + { + int clusterId = planeSuperCluster[i]; + SVzNL3DRangeD& a_roi = objClustersROIs[clusterId]; + if ((a_roi.yRange.min < bodyYRange.min) && (a_roi.yRange.max > bodyYRange.max)) //ڻYΧ + { + if(noseClusterId < 0) + noseClusterId = clusterId; + else if (objClustersROIs[noseClusterId].zRange.min > objClustersROIs[clusterId].zRange.min) + noseClusterId = clusterId; + } + } + if (noseClusterId < 0) + { + *errCode = SX_ERR_NOSEPOINT_FAIL; + return planePoseInfo; + } + //ǵ + SVzNL3DPosition nosePoint; + nosePoint.nPointIdx = 0; + nosePoint.pt3D = { 0.0, 0.0, 0.0 }; + for (int i = 0; i < (int)objClusters[noseClusterId].size(); i++) + { + if (nosePoint.pt3D.z < 1e-4) + nosePoint = objClusters[noseClusterId][i]; + else if(nosePoint.pt3D.z > objClusters[noseClusterId][i].pt3D.z) + nosePoint = objClusters[noseClusterId][i]; + } + if (nosePoint.pt3D.z < 1e-4) + { + *errCode = SX_ERR_NOSEPOINT_FAIL; + return planePoseInfo; + } +#if _OUTPUT_DEBUG_DATA + { + int nose_lineIdx = nosePoint.nPointIdx >> 16; + int nose_ptIdx = nosePoint.nPointIdx & 0xffff; + debugData[nose_lineIdx][nose_ptIdx].nPointIdx |= 0x10000; //ǵ + } +#endif + //̬ + //жϻڵijȣԻǸ߶Ϊ׼ȡ߶ȷΧڵĵ㣬㳤 + SVzNLRangeD bodyHRange = { nosePoint.pt3D.y + bodyRangeToNose.min, nosePoint.pt3D.y + bodyRangeToNose.max }; + std::vector bodyData; + for (int i = 0; i < (int)objClusters[noseClusterId].size(); i++) + { + if ((objClusters[noseClusterId][i].pt3D.y >= bodyHRange.min) && (objClusters[noseClusterId][i].pt3D.y <= bodyHRange.max)) + bodyData.push_back(objClusters[noseClusterId][i]); + } + //㵽 + double maxDistanceToNose = 0; + std::vector XOZBodayProjectionData; + std::vector XOZBodayData; + for (int i = 0; i < (int)bodyData.size(); i++) + { + double dist = sqrt(pow(nosePoint.pt3D.x - bodyData[i].pt3D.x, 2) + pow(nosePoint.pt3D.z - bodyData[i].pt3D.z, 2)); + if (maxDistanceToNose < dist) + maxDistanceToNose = dist; + + if ((dist >= sameRBodyRange.min) && (dist <= sameRBodyRange.max)) + { + XOZBodayData.push_back(bodyData[i]); + + SVzNL2DPointD a_projection = { bodyData[i].pt3D.x, bodyData[i].pt3D.z }; + XOZBodayProjectionData.push_back(a_projection); + } + } + if (maxDistanceToNose > bodyHeadJudge_lenTh) // + { + //ȡݣPCA̬ + // άPCAƽƷ + SVzNL2DPointD axis, centroid; + pca2D(XOZBodayProjectionData, axis, centroid); + if (axis.y < 0) + axis = { -axis.x, -axis.y }; + + double dirAngle = atan(axis.x / axis.y) * 180.0 / PI; + planePoseInfo.nosePoint = nosePoint.pt3D; + planePoseInfo.axis = { axis.x, 0, axis.y }; + planePoseInfo.axis = vec3_normalize(planePoseInfo.axis); + planePoseInfo.distance = nosePoint.pt3D.z - rotateParkingPoint.z + parkingParam.distFromNoseToWheel; + planePoseInfo.deviation = nosePoint.pt3D.x - rotateParkingPoint.x; //ƫ + planePoseInfo.dirAngle_deg = dirAngle; //Ƕ + +#if _OUTPUT_DEBUG_DATA + for(int m = 0; m <(int)XOZBodayData.size(); m ++) + { + int nose_lineIdx = XOZBodayData[m].nPointIdx >> 16; + int nose_ptIdx = XOZBodayData[m].nPointIdx & 0xffff; + if( (debugData[nose_lineIdx][nose_ptIdx].nPointIdx & 0xffff0000) == 0) + debugData[nose_lineIdx][nose_ptIdx].nPointIdx |= 0x20000; // + } +#endif + } + else //ͷ + { + //ȡҷ + std::vector> distanceValidData; + distanceValidData.resize(planeClusterSize); + for (int idx = 0; idx < planeClusterSize; idx++) + { + int clusterIdx = planeSuperCluster[idx]; + if (clusterIdx == noseClusterId) + continue; + + for (int i = 0; i < (int)objClusters[clusterIdx].size(); i++) + { + double dist = sqrt(pow(nosePoint.pt3D.x - objClusters[clusterIdx][i].pt3D.x, 2) + pow(nosePoint.pt3D.z - objClusters[clusterIdx][i].pt3D.z, 2)); + if ((dist >= engineToNoseDistRange.min) && (dist <= engineToNoseDistRange.max)) + distanceValidData[idx].push_back(objClusters[clusterIdx][i]); + } + } + //ROI + std::vector dataROIs; + std::vector validFlags; + dataROIs.resize(planeClusterSize); + validFlags.resize(planeClusterSize); + int validNum = 0; + for (int idx = 0; idx < planeClusterSize; idx++) + { + if (distanceValidData[idx].size() == 0) + { + validFlags[idx] = 0; + dataROIs[idx] = { 0.0, 0.0 }; + } + else + { + validFlags[idx] = 1; + validNum++; + SVzNLRangeD xRng = { distanceValidData[idx][0].pt3D.x,distanceValidData[idx][0].pt3D.x }; + for (int i = 1; i < (int)distanceValidData[idx].size(); i++) + { + xRng.min = xRng.min > distanceValidData[idx][i].pt3D.x ? distanceValidData[idx][i].pt3D.x : xRng.min; + xRng.max = xRng.max < distanceValidData[idx][i].pt3D.x ? distanceValidData[idx][i].pt3D.x : xRng.max; + } + dataROIs[idx] = xRng; + } + } + if (validNum < 2) + { + *errCode = SX_ERR_ENDINE_FAIL; + return planePoseInfo; + } + + //ȡEngine + int leftEngineIdx = -1; + int rightEngineIdx = -1; + for (int idx = 0; idx < planeClusterSize; idx++) + { + if (validFlags[idx] == 0) + continue; + + if (leftEngineIdx < 0) + leftEngineIdx = idx; + else if (dataROIs[leftEngineIdx].min > dataROIs[idx].min) + leftEngineIdx = idx; + + if (rightEngineIdx < 0) + rightEngineIdx = idx; + else if (dataROIs[rightEngineIdx].max < dataROIs[idx].max) + rightEngineIdx = idx; + } + + if (leftEngineIdx == rightEngineIdx) + { + *errCode = SX_ERR_ENDINE_FAIL; + return planePoseInfo; + } + + //ȡҵ㼯͵㣬ΪҷIJο + SVzNL3DPosition leftEnginePoint = distanceValidData[leftEngineIdx][0]; + for (int i = 1; i < (int)distanceValidData[leftEngineIdx].size(); i++) + { + if (leftEnginePoint.pt3D.y < distanceValidData[leftEngineIdx][i].pt3D.y) + leftEnginePoint = distanceValidData[leftEngineIdx][i]; + } + SVzNL3DPosition rightEnginePoint = distanceValidData[rightEngineIdx][0]; + for (int i = 1; i < (int)distanceValidData[rightEngineIdx].size(); i++) + { + if (rightEnginePoint.pt3D.y < distanceValidData[rightEngineIdx][i].pt3D.y) + rightEnginePoint = distanceValidData[rightEngineIdx][i]; + } +#if _OUTPUT_DEBUG_DATA + { + int nose_lineIdx = leftEnginePoint.nPointIdx >> 16; + int nose_ptIdx = leftEnginePoint.nPointIdx & 0xffff; + debugData[nose_lineIdx][nose_ptIdx].nPointIdx |= 0x40000; //ǵ + + nose_lineIdx = rightEnginePoint.nPointIdx >> 16; + nose_ptIdx = rightEnginePoint.nPointIdx & 0xffff; + debugData[nose_lineIdx][nose_ptIdx].nPointIdx |= 0x40000; //ǵ + } +#endif + //̬ + //axisҷֱ, (-y,, x)ʽ + SVzNL2DPointD axis = { -(rightEnginePoint.pt3D.z - leftEnginePoint.pt3D.z), rightEnginePoint.pt3D.x - leftEnginePoint.pt3D.x }; + if (axis.y < 0) + axis = { -axis.x, -axis.y}; + + double dirAngle = atan(axis.x / axis.y) * 180.0 / PI; + planePoseInfo.nosePoint = nosePoint.pt3D; + planePoseInfo.axis = { axis.x, 0, axis.y }; + planePoseInfo.axis = vec3_normalize(planePoseInfo.axis); + planePoseInfo.distance = nosePoint.pt3D.z - rotateParkingPoint.z + parkingParam.distFromNoseToWheel; + planePoseInfo.deviation = nosePoint.pt3D.x - rotateParkingPoint.x; //ƫ + planePoseInfo.dirAngle_deg = dirAngle; //Ƕ + } + + return planePoseInfo; +} + diff --git a/sourceCode/planeLocalization_Export.h b/sourceCode/planeLocalization_Export.h new file mode 100644 index 0000000..961b0b7 --- /dev/null +++ b/sourceCode/planeLocalization_Export.h @@ -0,0 +1,44 @@ +#pragma once + +#include "SG_algo_Export.h" +#include + +#define _OUTPUT_DEBUG_DATA 1 + +typedef struct +{ + double distance; //ͣ + double deviation; //ƫ߾ + double dirAngle_deg; //ǰǣ ԶΪλ + SVzNL3DPoint nosePoint; + SVzNL3DPoint axis; +}SSX_planeInfo; + +typedef struct +{ + SVzNL3DPoint parkingPoint; //ͣ㣨辭ƽ + SVzNL3DPoint guideLinePoint; //ͣϵ㣨辭ƽȷ߷򡣷Ϊͣ㵽ϵߡͣ20m + double guidingRange; //ɻΧ ͣΪ׼˷ΧĿ겻 + double parkingRange; //ɻ߾룬ڹЧĿ꣬ Ϊ׼ + double distFromNoseToWheel; //ͣΪӵص㣬˲ڼͣλ +}SSX_planeParkingParam; + +//汾 +SG_APISHARED_EXPORT const char* wd_PlaneLocalizationVersion(void); + +//ˮƽװƽ +//ZƽеʱҪԵΪգˮƽ +//תΪƽƽ淨ΪֱIJ +SG_APISHARED_EXPORT SSG_planeCalibPara wd_getGroundCalibPara( + std::vector< std::vector>& scanLines); + +//ɻ̬ +SG_APISHARED_EXPORT SSX_planeInfo wd_planeLocalization( + std::vector< std::vector>& scanLines, + const SSG_planeCalibPara groundCalibParam, + const SSX_planeParkingParam parkingParam, + const SSG_treeGrowParam growParam, +#if _OUTPUT_DEBUG_DATA + std::vector< std::vector>& debugData, +#endif + int* errCode);