diff --git a/SG_Algorithm.sln b/SG_Algorithm.sln index ab6ab62..85ce659 100644 --- a/SG_Algorithm.sln +++ b/SG_Algorithm.sln @@ -139,6 +139,21 @@ Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "binocularMarkCam_test", "Bi {AB25A4FE-6457-4CB4-A190-EBEF603335C7} = {AB25A4FE-6457-4CB4-A190-EBEF603335C7} EndProjectSection EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "gasFillingPortPosition", "gasFillingPortPosition\gasFillingPortPosition.vcxproj", "{5367493C-9799-4684-A809-23B6EF40C70A}" + ProjectSection(ProjectDependencies) = postProject + {95DC3F1A-902A-490E-BD3B-B10463CF0EBD} = {95DC3F1A-902A-490E-BD3B-B10463CF0EBD} + EndProjectSection +EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "gasFillingPortPosition_test", "gasFillingPortPosition_test\gasFillingPortPosition_test.vcxproj", "{1EF990A1-9CCB-492A-B6CB-5C233300826B}" + ProjectSection(ProjectDependencies) = postProject + {95DC3F1A-902A-490E-BD3B-B10463CF0EBD} = {95DC3F1A-902A-490E-BD3B-B10463CF0EBD} + {5367493C-9799-4684-A809-23B6EF40C70A} = {5367493C-9799-4684-A809-23B6EF40C70A} + EndProjectSection +EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "BQ_assemblyPosition", "BQ_assemblyPosition\BQ_assemblyPosition.vcxproj", "{0E62EEE4-ABB5-4364-A794-CCFFE8815426}" +EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "BQ_assemblyPosition_test", "BQ_assemblyPosition_test\BQ_assemblyPosition_test.vcxproj", "{BC38D1E5-10CB-438B-AC72-6012303CE139}" +EndProject Global GlobalSection(SolutionConfigurationPlatforms) = preSolution Debug|x64 = Debug|x64 @@ -363,6 +378,38 @@ Global {FB3BCF92-DF8B-4CEC-AA39-4ECD31F82718}.Release|x64.Build.0 = Release|x64 {FB3BCF92-DF8B-4CEC-AA39-4ECD31F82718}.Release|x86.ActiveCfg = Release|Win32 {FB3BCF92-DF8B-4CEC-AA39-4ECD31F82718}.Release|x86.Build.0 = Release|Win32 + {5367493C-9799-4684-A809-23B6EF40C70A}.Debug|x64.ActiveCfg = Debug|x64 + {5367493C-9799-4684-A809-23B6EF40C70A}.Debug|x64.Build.0 = Debug|x64 + {5367493C-9799-4684-A809-23B6EF40C70A}.Debug|x86.ActiveCfg = Debug|Win32 + {5367493C-9799-4684-A809-23B6EF40C70A}.Debug|x86.Build.0 = Debug|Win32 + {5367493C-9799-4684-A809-23B6EF40C70A}.Release|x64.ActiveCfg = Release|x64 + {5367493C-9799-4684-A809-23B6EF40C70A}.Release|x64.Build.0 = Release|x64 + {5367493C-9799-4684-A809-23B6EF40C70A}.Release|x86.ActiveCfg = Release|Win32 + {5367493C-9799-4684-A809-23B6EF40C70A}.Release|x86.Build.0 = Release|Win32 + {1EF990A1-9CCB-492A-B6CB-5C233300826B}.Debug|x64.ActiveCfg = Debug|x64 + {1EF990A1-9CCB-492A-B6CB-5C233300826B}.Debug|x64.Build.0 = Debug|x64 + {1EF990A1-9CCB-492A-B6CB-5C233300826B}.Debug|x86.ActiveCfg = Debug|Win32 + {1EF990A1-9CCB-492A-B6CB-5C233300826B}.Debug|x86.Build.0 = Debug|Win32 + {1EF990A1-9CCB-492A-B6CB-5C233300826B}.Release|x64.ActiveCfg = Release|x64 + {1EF990A1-9CCB-492A-B6CB-5C233300826B}.Release|x64.Build.0 = Release|x64 + {1EF990A1-9CCB-492A-B6CB-5C233300826B}.Release|x86.ActiveCfg = Release|Win32 + {1EF990A1-9CCB-492A-B6CB-5C233300826B}.Release|x86.Build.0 = Release|Win32 + {0E62EEE4-ABB5-4364-A794-CCFFE8815426}.Debug|x64.ActiveCfg = Debug|x64 + {0E62EEE4-ABB5-4364-A794-CCFFE8815426}.Debug|x64.Build.0 = Debug|x64 + {0E62EEE4-ABB5-4364-A794-CCFFE8815426}.Debug|x86.ActiveCfg = Debug|Win32 + {0E62EEE4-ABB5-4364-A794-CCFFE8815426}.Debug|x86.Build.0 = Debug|Win32 + {0E62EEE4-ABB5-4364-A794-CCFFE8815426}.Release|x64.ActiveCfg = Release|x64 + {0E62EEE4-ABB5-4364-A794-CCFFE8815426}.Release|x64.Build.0 = Release|x64 + {0E62EEE4-ABB5-4364-A794-CCFFE8815426}.Release|x86.ActiveCfg = Release|Win32 + {0E62EEE4-ABB5-4364-A794-CCFFE8815426}.Release|x86.Build.0 = Release|Win32 + {BC38D1E5-10CB-438B-AC72-6012303CE139}.Debug|x64.ActiveCfg = Debug|x64 + {BC38D1E5-10CB-438B-AC72-6012303CE139}.Debug|x64.Build.0 = Debug|x64 + {BC38D1E5-10CB-438B-AC72-6012303CE139}.Debug|x86.ActiveCfg = Debug|Win32 + {BC38D1E5-10CB-438B-AC72-6012303CE139}.Debug|x86.Build.0 = Debug|Win32 + {BC38D1E5-10CB-438B-AC72-6012303CE139}.Release|x64.ActiveCfg = Release|x64 + {BC38D1E5-10CB-438B-AC72-6012303CE139}.Release|x64.Build.0 = Release|x64 + {BC38D1E5-10CB-438B-AC72-6012303CE139}.Release|x86.ActiveCfg = Release|Win32 + {BC38D1E5-10CB-438B-AC72-6012303CE139}.Release|x86.Build.0 = Release|Win32 EndGlobalSection GlobalSection(SolutionProperties) = preSolution HideSolutionNode = FALSE diff --git a/gasFillingPortPosition/gasFillingPortPosition.vcxproj b/gasFillingPortPosition/gasFillingPortPosition.vcxproj new file mode 100644 index 0000000..df84305 --- /dev/null +++ b/gasFillingPortPosition/gasFillingPortPosition.vcxproj @@ -0,0 +1,172 @@ + + + + + Debug + Win32 + + + Release + Win32 + + + Debug + x64 + + + Release + x64 + + + + + + + + + + 16.0 + Win32Proj + {5367493c-9799-4684-a809-23b6ef40c70a} + gasFillingPortPosition + 10.0 + + + + DynamicLibrary + true + v142 + Unicode + + + DynamicLibrary + false + v142 + true + Unicode + + + DynamicLibrary + true + v142 + Unicode + + + DynamicLibrary + false + v142 + true + Unicode + + + + + + + + + + + + + + + + + + + + + true + + + false + + + true + $(SolutionDir)build\$(Platform)\$(Configuration)\ + ..\..\thirdParty\VzNLSDK\Inc;..\..\thirdParty\opencv320\build\include;..\sourceCode;..\sourceCode\inc;$(IncludePath) + + + false + $(SolutionDir)build\$(Platform)\$(Configuration)\ + ..\..\thirdParty\VzNLSDK\Inc;..\..\thirdParty\opencv320\build\include;..\sourceCode;..\sourceCode\inc;$(IncludePath) + + + + Level3 + true + WIN32;_DEBUG;GASFILLINGPORTPOSITION_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) + true + Use + pch.h + + + Windows + true + false + + + + + Level3 + true + true + true + WIN32;NDEBUG;GASFILLINGPORTPOSITION_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) + true + Use + pch.h + + + Windows + true + true + true + false + + + + + Level3 + true + _CRT_SECURE_NO_WARNINGS;_DEBUG;GASFILLINGPORTPOSITION_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;GASFILLINGPORTPOSITION_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) + true + NotUsing + pch.h + ..\..\thirdParty\opencv320\build\include;%(AdditionalIncludeDirectories) + + + Windows + true + true + true + false + ..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Release;%(AdditionalLibraryDirectories) + opencv_world320.lib;baseAlgorithm.lib;%(AdditionalDependencies) + + + + + + \ No newline at end of file diff --git a/gasFillingPortPosition_test/gasFillingPortPosition_test.cpp b/gasFillingPortPosition_test/gasFillingPortPosition_test.cpp new file mode 100644 index 0000000..ad3a51d --- /dev/null +++ b/gasFillingPortPosition_test/gasFillingPortPosition_test.cpp @@ -0,0 +1,293 @@ +// gasFillingPortPosition_test.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。 +// + +#include +#include +#include +#include +#include +#include "direct.h" +#include +#include "gasFillingPortPosition_Export.h" +#include +#include +#include + +typedef struct +{ + int r; + int g; + int b; +}SG_color; + +typedef struct +{ + int nPointIdx; + double x; + double y; + double z; + float r; + float g; + float b; +} SPointXYZRGB; + +void vzReadLaserScanPointFromFile_XYZ_vector(const char* fileName, std::vector>& scanData) +{ + std::ifstream inputFile(fileName); + std::string linedata; + + if (inputFile.is_open() == false) + return; + + std::vector< SVzNL3DPosition> a_line; + int ptIdx = 0; + while (getline(inputFile, linedata)) + { + if (0 == strncmp("Line_", linedata.c_str(), 5)) + { + int ptSize = (int)a_line.size(); + if (ptSize > 0) + { + scanData.push_back(a_line); + } + a_line.clear(); + ptIdx = 0; + } + else if (0 == strncmp("{", linedata.c_str(), 1)) + { + float X, Y, Z; + int imageY = 0; + float leftX, leftY; + float rightX, rightY; + sscanf_s(linedata.c_str(), "{%f,%f,%f}-{%f,%f}-{%f,%f}", &X, &Y, &Z, &leftX, &leftY, &rightX, &rightY); + SVzNL3DPosition a_pt; + a_pt.pt3D.x = X; + a_pt.pt3D.y = Y; + a_pt.pt3D.z = Z; + a_pt.nPointIdx = ptIdx; + ptIdx++; + a_line.push_back(a_pt); + } + } + //last line + int ptSize = (int)a_line.size(); + if (ptSize > 0) + { + scanData.push_back(a_line); + a_line.clear(); + } + + inputFile.close(); + return; +} + +void _outputFillingPortInfo(char* fileName, SSG_6DOF centerPose) +{ + std::ofstream sw(fileName); + + char dataStr[250]; + sw << "A:" << std::endl; + sprintf_s(dataStr, 250, "中心点: (%g, %g, %g)", centerPose.x, centerPose.y, centerPose.z); + sw << dataStr << std::endl; + sprintf_s(dataStr, 250, "法向量: (%g, %g, %g)", centerPose.x_roll, centerPose.y_pitch, centerPose.z_yaw); + sw << dataStr << std::endl; + sw.close(); +} + +void _outputRGBDScan_fillingPort_RGBD( + char* fileName, + std::vector>& scanLines, + SSG_6DOF centerPose) +{ + int lineNum = (int)scanLines.size(); + std::ofstream sw(fileName); + int realLines = lineNum; + if (centerPose.z > 1e-4) + realLines++; + + sw << "LineNum:" << realLines << std::endl; + sw << "DataType: 0" << std::endl; + sw << "ScanSpeed: 0" << std::endl; + sw << "PointAdjust: 1" << std::endl; + sw << "MaxTimeStamp: 0_0" << std::endl; + + int maxLineIndex = 0; + int max_stamp = 0; + + SG_color rgb = { 0, 0, 0 }; + + SG_color objColor[8] = { + {245,222,179},//淡黄色 + {210,105, 30},//巧克力色 + {240,230,140},//黄褐色 + {135,206,235},//天蓝色 + {250,235,215},//古董白 + {189,252,201},//薄荷色 + {221,160,221},//梅红色 + {188,143,143},//玫瑰红色 + }; + int size = 1; + int lineIdx = 0; + for (int line = 0; line < lineNum; line++) + { + int linePtNum = (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; + if(pt3D->nPointIdx == 0) + { + rgb = { 200, 200, 200 }; + size = 1; + } + else + { + rgb = { 255, 97, 0 }; + size = 3; + } + 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 (centerPose.z > 1e-4) + { + std::vector ptBuffer; + SVzNL3DPoint cpt = { centerPose.x, centerPose.y, centerPose.z }; + ptBuffer.push_back(cpt); + + int linePtNum = (int)ptBuffer.size(); + sw << "Line_" << lineNum << "_0_" << linePtNum + 1 << std::endl; + lineNum++; + + rgb = { 255, 0, 0 }; + size = 15; + for (int j = 0; j < linePtNum; j++) + { + float x = (float)ptBuffer[j].x; + float y = (float)ptBuffer[j].y; + float z = (float)ptBuffer[j].z; + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; + } + //加一个点，用于跳过显示工具bug + float x = (float)ptBuffer[0].x; + float y = (float)ptBuffer[0].y; + float z = (float)ptBuffer[0].z; + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; + + size = 1; + double len = 60; + SVzNL3DPoint pt0 = { cpt.x + len * centerPose.x_roll, + cpt.y + len * centerPose.y_pitch, + cpt.z + len * centerPose.z_yaw }; + SVzNL3DPoint pt1 = { cpt.x - len * centerPose.x_roll, + cpt.y - len * centerPose.y_pitch, + cpt.z - len * centerPose.z_yaw }; + //显示法向量 + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << (float)pt0.x << "," << (float)pt0.y << "," << (float)pt0.z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << pt1.x << "," << pt1.y << "," << pt1.z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + lineIdx++; + } + sw.close(); +} + +#define TEST_GROUP 1 +int main() +{ + const char* dataPath[TEST_GROUP] = { + "F:/ShangGu/项目/冠钦_LNG自动加气/加气口测试数据/", //0 + }; + + SVzNLRange fileIdx[TEST_GROUP] = { + {2,30}, + }; + + const char* ver = wd_gasFillingPortPositionVersion(); + printf("ver:%s\n", ver); + + for (int grp = 0; grp < 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_Hi229156.txt", dataPath[grp], fidx); + + std::vector> scanLines; + vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines); + + //转成plyTxt格式 + //sprintf_s(_scan_file, "%s%d_ply_Hi229229.txt", dataPath[grp], fidx); + //wdSavePlyTxt(_scan_file, scanLines); + + long t1 = (long)GetTickCount64();//统计时间 + + SSX_gasFillingPortPara gasFillingPortPara; + gasFillingPortPara.innerD = 42.0; + gasFillingPortPara.outerD = 52.0; + + SSG_lineSegParam lineSegPara; + lineSegPara.maxDist = 1.0; + lineSegPara.segGapTh_y = 5.0; //y方向间隔大于5mm认为是分段 + lineSegPara.segGapTh_z = 10.0; //z方向间隔大于10mm认为是分段 + + SSG_outlierFilterParam filterParam; + filterParam.continuityTh = 20.0; //噪声滤除。当相邻点的z跳变大于此门限时，检查是否为噪声。若长度小于outlierLen， 视为噪声 + filterParam.outlierTh = 5; + + SSG_treeGrowParam growParam; + growParam.maxLineSkipNum = 10; + growParam.yDeviation_max = 5.0; + growParam.maxSkipDistance = 5.0; + growParam.zDeviation_max = 2.0;// + growParam.minLTypeTreeLen = 100; //mm + growParam.minVTypeTreeLen = 100; //mm + + int errCode = 0; + SSG_6DOF centerPose = wd_getGasFillingPortPosition( + scanLines, + gasFillingPortPara, + lineSegPara, + filterParam, + growParam, + &errCode); + long t2 = (long)GetTickCount64(); + printf("%s: %d(ms)!\n", _scan_file, (int)(t2 - t1)); + //输出测试结果 + sprintf_s(_scan_file, "%sresult\\%d_result.txt", dataPath[grp], fidx); + _outputRGBDScan_fillingPort_RGBD(_scan_file, scanLines, centerPose); + sprintf_s(_scan_file, "%sresult\\%d_fillingPort_info.txt", dataPath[grp], fidx); + _outputFillingPortInfo(_scan_file, centerPose); + } + } +} + +// 运行程序: Ctrl + F5 或调试 >“开始执行(不调试)”菜单 +// 调试程序: F5 或调试 >“开始调试”菜单 + +// 入门使用技巧: +// 1. 使用解决方案资源管理器窗口添加/管理文件 +// 2. 使用团队资源管理器窗口连接到源代码管理 +// 3. 使用输出窗口查看生成输出和其他消息 +// 4. 使用错误列表窗口查看错误 +// 5. 转到“项目”>“添加新项”以创建新的代码文件，或转到“项目”>“添加现有项”以将现有代码文件添加到项目 +// 6. 将来，若要再次打开此项目，请转到“文件”>“打开”>“项目”并选择 .sln 文件 diff --git a/gasFillingPortPosition_test/gasFillingPortPosition_test.vcxproj b/gasFillingPortPosition_test/gasFillingPortPosition_test.vcxproj new file mode 100644 index 0000000..aa3d531 --- /dev/null +++ b/gasFillingPortPosition_test/gasFillingPortPosition_test.vcxproj @@ -0,0 +1,157 @@ + + + + + Debug + Win32 + + + Release + Win32 + + + Debug + x64 + + + Release + x64 + + + + 16.0 + Win32Proj + {1ef990a1-9ccb-492a-b6cb-5c233300826b} + gasFillingPortPositiontest + 10.0 + + + + Application + true + v142 + Unicode + + + Application + false + v142 + true + Unicode + + + Application + true + v142 + Unicode + + + Application + false + v142 + true + Unicode + + + + + + + + + + + + + + + + + + + + + true + + + false + + + true + $(SolutionDir)build\$(Platform)\$(Configuration)\ + ..\..\thirdParty\VzNLSDK\Inc;..\sourceCode;..\sourceCode\inc;$(IncludePath) + + + false + $(SolutionDir)build\$(Platform)\$(Configuration)\ + ..\..\thirdParty\VzNLSDK\Inc;..\sourceCode;..\sourceCode\inc;$(IncludePath) + + + + Level3 + true + WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + + + Console + true + + + + + Level3 + true + true + true + WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + + + Console + true + true + true + + + + + Level3 + true + _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;gasFillingPortPosition.lib;%(AdditionalDependencies) + + + + + Level3 + true + true + true + _CRT_SECURE_NO_WARNINGS;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + ..\..\thirdParty\opencv320\build\include; + + + Console + true + true + true + ..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Release;%(AdditionalLibraryDirectories) + opencv_world320.lib;gasFillingPortPosition.lib;%(AdditionalDependencies) + + + + + + + + + \ No newline at end of file diff --git a/sourceCode/SG_baseAlgo_Export.h b/sourceCode/SG_baseAlgo_Export.h index 6f4c7df..77a0ba9 100644 --- a/sourceCode/SG_baseAlgo_Export.h +++ b/sourceCode/SG_baseAlgo_Export.h @@ -102,6 +102,14 @@ SG_APISHARED_EXPORT void wd_getRingArcFeature( std::vector& line_ringArcs // ); +//ֱȡsplit-and-merge˼򻯣յֱߴֱ +SG_APISHARED_EXPORT void wd_simpleLineSegment( + std::vector< SVzNL3DPosition>& lineData, + int lineIdx, + SVzNLRangeD lineLenRange, + const SSG_lineSegParam lineSegPara, + std::vector& lineSegs); + /// /// ȡϵJumping /// nPointIdx¶Feature @@ -345,6 +353,12 @@ SG_APISHARED_EXPORT void lineFitting_abc( double* _b, double* _c); +//ԲС +SG_APISHARED_EXPORT double fitCircleByLeastSquare( + const std::vector& pointArray, + SVzNL3DPoint& center, + double& radius); + //Zֵ SG_APISHARED_EXPORT double computeMeanZ(std::vector< SVzNL3DPoint>& pts); @@ -412,6 +426,12 @@ SG_APISHARED_EXPORT void SG_TwoPassLabel( std::vector& labelRgns, int connectivity = 4); +//: z = Ax + By + C +//res: [0]=A, [1]= B, [2]=-1.0, [3]=C, +SG_APISHARED_EXPORT void vzCaculateLaserPlane( + std::vector Points3ds, + std::vector& res); + //һƽƽ //пһƽͲοƽƽ棬ߵƽеƽ //תΪƽƽ淨ΪֱIJ @@ -419,6 +439,11 @@ SG_APISHARED_EXPORT SSG_planeCalibPara sg_getPlaneCalibPara( SVzNL3DLaserLine* laser3DPoints, int lineNum); +//һƽΪˮƽƽ(zΪ̶ֵ +SG_APISHARED_EXPORT SSG_planeCalibPara adjustPlaneToXYPlane( + double plane_A, double plane_B, double plane_C //ƽ淨 +); + //һƽƽ //пһƽͲοƽƽ棬ߵƽеƽ //תΪƽƽ淨ΪֱIJ @@ -460,12 +485,25 @@ SG_APISHARED_EXPORT void lineDataRT_RGBD( const double* camPoseR, double groundH); +//ڵľ SG_APISHARED_EXPORT void sg_pointClustering( std::vector< SVzNL3DPosition>& pts, double clusterDist, std::vector>& objClusters //result ); +//դϵĴڵڵľ࣬3D +//ʹvector2άṹ +SG_APISHARED_EXPORT void wd_pointClustering2D( + std::vector>& featureMask, + std::vector>& feature3DInfo, + int clusterCheckWin, // + SSG_treeGrowParam growParam,// + int clusterID, //ǰClusterID + std::vector< SVzNL2DPoint>& a_cluster, //result + SVzNL3DRangeD& clusterRoi //roi3D +); + //դݽXYƽϵͶӰĿհ׵вֵ SG_APISHARED_EXPORT void pointClout2DProjection( std::vector< std::vector>& gridScanData, diff --git a/sourceCode/SG_baseDataType.h b/sourceCode/SG_baseDataType.h index e45b0d5..6c05012 100644 --- a/sourceCode/SG_baseDataType.h +++ b/sourceCode/SG_baseDataType.h @@ -48,6 +48,17 @@ typedef struct int idx; }SSG_intPair; +typedef struct +{ + int featurType; + int featureIdx_v; + int featureIdx_h; + int clusterID; + int flag; + int lineIdx; + int ptIdx; +}SSG_featureClusteringInfo; + typedef struct { double left; @@ -157,6 +168,13 @@ typedef struct double jumpCornerTh_2; }SSG_cornerParam; +typedef struct +{ + double segGapTh_y; //yޡڴޣΪ + double segGapTh_z; //zޡڴޣΪ + double maxDist; //㷽ǵĴڱ +}SSG_lineSegParam; + typedef struct { double scale_angle; //㷽ǵĴڱ diff --git a/sourceCode/SG_baseFunc.cpp b/sourceCode/SG_baseFunc.cpp index 3e71236..442fbbb 100644 --- a/sourceCode/SG_baseFunc.cpp +++ b/sourceCode/SG_baseFunc.cpp @@ -279,6 +279,72 @@ void lineFitting_abc(std::vector< SVzNL3DPoint>& inliers, double* _a, double* _b return; } +//ԲС +double fitCircleByLeastSquare( + const std::vector& pointArray, + SVzNL3DPoint& center, + double& radius) +{ + int N = pointArray.size(); + if (N < 3) { + return std::numeric_limits::max(); + } + + double sumX = 0.0; + double sumY = 0.0; + double sumX2 = 0.0; + double sumY2 = 0.0; + double sumX3 = 0.0; + double sumY3 = 0.0; + double sumXY = 0.0; + double sumXY2 = 0.0; + double sumX2Y = 0.0; + + for (int pId = 0; pId < N; ++pId) { + sumX += pointArray[pId].x; + sumY += pointArray[pId].y; + + double x2 = pointArray[pId].x * pointArray[pId].x; + double y2 = pointArray[pId].y * pointArray[pId].y; + sumX2 += x2; + sumY2 += y2; + + sumX3 += x2 * pointArray[pId].x; + sumY3 += y2 * pointArray[pId].y; + sumXY += pointArray[pId].x * pointArray[pId].y; + sumXY2 += pointArray[pId].x * y2; + sumX2Y += x2 * pointArray[pId].y; + } + + double C, D, E, G, H; + double a, b, c; + + C = N * sumX2 - sumX * sumX; + D = N * sumXY - sumX * sumY; + E = N * sumX3 + N * sumXY2 - (sumX2 + sumY2) * sumX; + G = N * sumY2 - sumY * sumY; + H = N * sumX2Y + N * sumY3 - (sumX2 + sumY2) * sumY; + + a = (H * D - E * G) / (C * G - D * D); + b = (H * C - E * D) / (D * D - G * C); + c = -(a * sumX + b * sumY + sumX2 + sumY2) / N; + + center.x = -a / 2.0; + center.y = -b / 2.0; + radius = sqrt(a * a + b * b - 4 * c) / 2.0; + + double err = 0.0; + double e; + double r2 = radius * radius; + for (int pId = 0; pId < N; ++pId) { + e = pow(pointArray[pId].x - center.x,2) + pow(pointArray[pId].y - center.y, 2) - r2; + if (e > err) { + err = e; + } + } + return err; +} + //Zֵ double computeMeanZ(std::vector< SVzNL3DPoint>& pts) { @@ -1357,6 +1423,7 @@ SSG_planeCalibPara sg_getPlaneCalibPara( } //ƽ std::vector planceFunc; + //res: [0]=A, [1]= B, [2]=-1.0, [3]=C, vzCaculateLaserPlane(Points3ds, planceFunc); #if 1 //תתʹԪ @@ -1453,6 +1520,46 @@ SSG_planeCalibPara sg_getPlaneCalibPara( return planePara; } +SSG_planeCalibPara adjustPlaneToXYPlane(double plane_A, double plane_B, double plane_C) +{ + SSG_planeCalibPara calibPara; + + //תתʹԪ + Vector3 a = Vector3(plane_A, plane_B, plane_C); + Vector3 b = Vector3(0, 0, -1.0); + Quaternion quanPara = rotationBetweenVectors(a, b); + + RotationMatrix rMatrix; + quaternionToMatrix(quanPara, rMatrix.data); + //㷴ת + Quaternion invQuanPara = rotationBetweenVectors(b, a); + RotationMatrix invMatrix; + quaternionToMatrix(invQuanPara, invMatrix.data); + + + calibPara.planeCalib[0] = rMatrix.data[0][0]; + calibPara.planeCalib[1] = rMatrix.data[0][1]; + calibPara.planeCalib[2] = rMatrix.data[0][2]; + calibPara.planeCalib[3] = rMatrix.data[1][0]; + calibPara.planeCalib[4] = rMatrix.data[1][1]; + calibPara.planeCalib[5] = rMatrix.data[1][2]; + calibPara.planeCalib[6] = rMatrix.data[2][0]; + calibPara.planeCalib[7] = rMatrix.data[2][1]; + calibPara.planeCalib[8] = rMatrix.data[2][2]; + + calibPara.invRMatrix[0] = invMatrix.data[0][0]; + calibPara.invRMatrix[1] = invMatrix.data[0][1]; + calibPara.invRMatrix[2] = invMatrix.data[0][2]; + calibPara.invRMatrix[3] = invMatrix.data[1][0]; + calibPara.invRMatrix[4] = invMatrix.data[1][1]; + calibPara.invRMatrix[5] = invMatrix.data[1][2]; + calibPara.invRMatrix[6] = invMatrix.data[2][0]; + calibPara.invRMatrix[7] = invMatrix.data[2][1]; + calibPara.invRMatrix[8] = invMatrix.data[2][2]; + + return calibPara; +} + //һƽƽ //пһƽͲοƽƽ棬ߵƽеƽ //תΪƽƽ淨ΪֱIJ diff --git a/sourceCode/SG_clustering.cpp b/sourceCode/SG_clustering.cpp index ac546bd..f47f8c2 100644 --- a/sourceCode/SG_clustering.cpp +++ b/sourceCode/SG_clustering.cpp @@ -66,4 +66,76 @@ void sg_pointClustering( } return; } -; \ No newline at end of file + +void wd_pointClustering2D( + std::vector>& featureMask, + std::vector>& feature3DInfo, + int clusterCheckWin, // + SSG_treeGrowParam growParam,// + int clusterID, //ǰClusterID + std::vector< SVzNL2DPoint>& a_cluster, //result + SVzNL3DRangeD& clusterRoi //roi3D +) +{ + int i = 0; + int lineNum = (int)featureMask.size(); + int linePtNum = (int)featureMask[0].size(); + while (1) + { + if (i >= a_cluster.size()) + break; + + SVzNL2DPoint a_seedPos = a_cluster[i]; + if ((a_seedPos.x == 390) && (a_seedPos.y == 949)) + int kkk = 1; + + SSG_featureClusteringInfo& a_seed = featureMask[a_seedPos.x][a_seedPos.y]; + SVzNL3DPoint& seedValue = feature3DInfo[a_seedPos.x][a_seedPos.y]; + if (0 == a_seed.clusterID) //clusterID == 0, δ + { + for (int i = -clusterCheckWin; i <= clusterCheckWin; i++) + { + for (int j = -clusterCheckWin; j <= clusterCheckWin; j++) + { + int y = j + a_seedPos.y; + int x = i + a_seedPos.x; + if ((x == 390) && (y == 949)) + int kkk = 1; + + if ((x >= 0) && (x < lineNum) && (y >= 0) && (y < linePtNum)) + { + SSG_featureClusteringInfo& chk_seed = featureMask[x][y]; + if ((chk_seed.featurType ==0) || (chk_seed.clusterID > 0)) //ֻδ + continue; + + SVzNL3DPoint& chkValue = feature3DInfo[x][y];; + double y_diff = abs(seedValue.y - chkValue.y); + double z_diff = abs(seedValue.z - chkValue.z); + double x_diff = abs(seedValue.x - chkValue.x); + if ((y_diff < growParam.yDeviation_max) && (z_diff < growParam.zDeviation_max) && + (x_diff < growParam.maxSkipDistance)) + { + if (0 == chk_seed.flag)//ֹظ + { + chk_seed.flag = 1; + SVzNL2DPoint new_seed = { x, y }; + a_cluster.push_back(new_seed); + } + } + } + + } + } + } + a_seed.clusterID = clusterID; + //ROI + clusterRoi.xRange.min = clusterRoi.xRange.min > seedValue.x ? seedValue.x : clusterRoi.xRange.min; + clusterRoi.xRange.max = clusterRoi.xRange.max < seedValue.x ? seedValue.x : clusterRoi.xRange.max; + clusterRoi.yRange.min = clusterRoi.yRange.min > seedValue.y ? seedValue.y : clusterRoi.yRange.min; + clusterRoi.yRange.max = clusterRoi.yRange.max < seedValue.y ? seedValue.y : clusterRoi.yRange.max; + clusterRoi.zRange.min = clusterRoi.zRange.min > seedValue.z ? seedValue.z : clusterRoi.zRange.min; + clusterRoi.zRange.max = clusterRoi.zRange.max < seedValue.z ? seedValue.z : clusterRoi.zRange.max; + i++; + } + return; +} \ No newline at end of file diff --git a/sourceCode/SG_lineFeature.cpp b/sourceCode/SG_lineFeature.cpp index 194ed11..5d26e02 100644 --- a/sourceCode/SG_lineFeature.cpp +++ b/sourceCode/SG_lineFeature.cpp @@ -2558,6 +2558,170 @@ void wd_getRingArcFeature( return; } +// С㼯Сڴ޷ֱߣ +const int MIN_POINT_COUNT = 5; +//ʹö˵ֱߣ㵽ֱߵľ룬޵ķָ +void split( + SSG_RUN a_run, + std::vector< SVzNL3DPosition>& lineData, + const double maxError, + std::vector< SSG_RUN>& lineSegs) +{ + if (a_run.len < MIN_POINT_COUNT) + return; + + int start = a_run.start; + int end = a_run.start + a_run.len - 1; + SVzNL3DPoint pt1 = lineData[start].pt3D; + SVzNL3DPoint pt2 = lineData[end].pt3D; + if ((pt1.z < 1e-4) || (pt2.z < 1e-4)) + return; + double _a, _b, _c; + compute2ptLine(pt1, pt2, &_a, &_b, &_c); + double denominator = sqrt(_a * _a + _b * _b); + //һ + _a = _a / denominator; + _b = _b / denominator; + _c = _c / denominator; + + double maxDist = 0; + double maxPos = 0; + for (int i = start; i <= end; i++) + { + SVzNL3DPoint a_pt = lineData[i].pt3D; + if (a_pt.z > 1e-4) + { + double dist = abs(a_pt.x * _a + a_pt.y * _b + _c); + if (maxDist < dist) + { + maxDist = dist; + maxPos = i; + } + } + } + if (maxDist < maxError) + { + // Ҫ󣬼߶μ + lineSegs.push_back(a_run); + return; + } + SSG_RUN run_1 = { a_run.start, maxPos-a_run.start + 1, 0 }; + SSG_RUN run_2 = { maxPos, a_run.start + a_run.len - maxPos, 0 }; + split(run_1, lineData, maxError, lineSegs); + split(run_2, lineData, maxError, lineSegs); +} + +//ֱȡsplit-and-merge˼򻯣յֱߴֱ +void wd_simpleLineSegment( + std::vector< SVzNL3DPosition>& lineData, + int lineIdx, + SVzNLRangeD lineLenRange, + const SSG_lineSegParam lineSegPara, + std::vector&lineSegs) +{ + int dataSize = (int)lineData.size(); + //ȥ + std::vector segs; + int runIdx = 1; + SSG_RUN a_run = { 0, -1, 0 }; //startIdx, len, lastIdx + double pre_z = 0; + double pre_y = 0; + for (int i = 0; i < dataSize; i++) + { + if (lineData[i].pt3D.z > 1e-4) + { + if (a_run.len < 0) + { + a_run.start = i; + a_run.len = 1; + a_run.value = i; + } + else + { + double z_diff = abs(lineData[i].pt3D.z - pre_z); + double y_diff = abs(lineData[i].pt3D.y - pre_y); + if ((z_diff < lineSegPara.segGapTh_z) && (y_diff < lineSegPara.segGapTh_y)) + { + a_run.len = i - a_run.start + 1; + a_run.value = i; + } + else + { + a_run.value = runIdx; + segs.push_back(a_run); + runIdx++; + + a_run.start = i; + a_run.len = 1; + a_run.value = i; + } + } + pre_z = lineData[i].pt3D.z; + pre_y = lineData[i].pt3D.y; + } + } + if (a_run.len > 0) + segs.push_back(a_run); + + //δ + int segSize = (int)segs.size(); + for (int si = 0; si < segSize; si++) + { + std::vector< SSG_RUN> segmentationLines; + split(segs[si], lineData, lineSegPara.maxDist, segmentationLines); + //תSSG_featureSemiCircleʽ + for (int m = 0, m_max = (int)segmentationLines.size(); m < m_max; m++) + { + SSG_featureSemiCircle a_seg; + a_seg.lineIdx = lineIdx; + a_seg.startPtIdx = segmentationLines[m].start; + a_seg.endPtIdx = segmentationLines[m].start + segmentationLines[m].len - 1; + + SVzNL3DPoint ptStart = lineData[a_seg.startPtIdx].pt3D; + SVzNL3DPoint ptEnd = lineData[a_seg.endPtIdx].pt3D; + double len = sqrt(pow(ptStart.x - ptEnd.x, 2) + pow(ptStart.y - ptEnd.y, 2) + pow(ptStart.z - ptEnd.z, 2)); + if ((len >= lineLenRange.min) && (len <= lineLenRange.max)) + { + int midPtIdx = (a_seg.startPtIdx + a_seg.endPtIdx) / 2; + bool validMid = true; + if (lineData[midPtIdx].pt3D.z < 1e-4) + { + validMid = false; + int chkWin = 1; + while (1) + { + int idx = midPtIdx + chkWin; + if (idx >= a_seg.endPtIdx) + break; + if (lineData[idx].pt3D.z > 1e-4) + { + midPtIdx = idx; + validMid = true; + break; + } + idx = midPtIdx - chkWin; + if (idx <= a_seg.startPtIdx) + break; + if (lineData[idx].pt3D.z > 1e-4) + { + midPtIdx = idx; + validMid = true; + break; + } + chkWin++; + } + } + if (true == validMid) + { + a_seg.midPtIdx = midPtIdx; + a_seg.midPt = lineData[midPtIdx].pt3D; + lineSegs.push_back(a_seg); + } + } + } + } +} + bool compareByIdx(const SSG_pntDirAngle& a, const SSG_pntDirAngle& b) { return a.pntIdx < b.pntIdx; } diff --git a/sourceCode/gasFillingPortPosition.cpp b/sourceCode/gasFillingPortPosition.cpp new file mode 100644 index 0000000..bf907b3 --- /dev/null +++ b/sourceCode/gasFillingPortPosition.cpp @@ -0,0 +1,326 @@ +#include +#include "SG_baseDataType.h" +#include "SG_baseAlgo_Export.h" +#include "gasFillingPortPosition_Export.h" +#include +#include + +//汾 +//version 1.0.0 : base version release to customer +std::string m_strVersion = "1.0.0"; +const char* wd_gasFillingPortPositionVersion(void) +{ + return m_strVersion.c_str(); +} + +//ȡλ +SSG_6DOF wd_getGasFillingPortPosition( + std::vector< std::vector>& scanLines, + const SSX_gasFillingPortPara gasFillingPortPara, + const SSG_lineSegParam lineSegPara, + const SSG_outlierFilterParam filterParam, + SSG_treeGrowParam growParam, + int* errCode) +{ + *errCode = 0; + SSG_6DOF resultPose; + memset(&resultPose, 0, sizeof(SSG_6DOF)); + + int lineNum = (int)scanLines.size(); + int linePtNum = (int)scanLines[0].size(); + bool isGridData = true; + //ȡֱ߶ + //ֱȡ + SVzNLRangeD lineLenRange; + lineLenRange.max = 1.2 * (gasFillingPortPara.outerD - gasFillingPortPara.innerD); + lineLenRange.min = 0.5 * (gasFillingPortPara.outerD - gasFillingPortPara.innerD) / 2; + std::vector> lineFeatures_v_raw; + for (int line = 0; line < lineNum; line++) + { + if (line == 390) + int kkk = 1; + + std::vector& lineData = scanLines[line]; + if (linePtNum != (int)lineData.size()) + isGridData = false; + + //˲˳쳣 + sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam); + + std::vector line_features; + int dataSize = (int)lineData.size(); + wd_simpleLineSegment( + lineData, + line, + lineLenRange, + lineSegPara, + line_features); + lineFeatures_v_raw.push_back(line_features); + } + + if (false == isGridData)//ݲʽ + { + *errCode = SG_ERR_NOT_GRID_FORMAT; + return resultPose; + } + + //ˮƽɨ + std::vector> hLines_raw; + hLines_raw.resize(linePtNum); + for (int i = 0; i < linePtNum; i++) + hLines_raw[i].resize(lineNum); + for (int line = 0; line < lineNum; line++) + { + for (int j = 0; j < linePtNum; j++) + { + scanLines[line][j].nPointIdx = 0; //ԭʼݵ0תʹã + hLines_raw[j][line] = scanLines[line][j]; + hLines_raw[j][line].pt3D.x = scanLines[line][j].pt3D.y; + hLines_raw[j][line].pt3D.y = scanLines[line][j].pt3D.x; + } + } + //ˮƽarcȡ + std::vector> lineFeatures_h_raw; + int lineNum_h_raw = (int)hLines_raw.size(); + for (int line = 0; line < lineNum_h_raw; line++) + { + if (line == 974) + int kkk = 1; + std::vector& lineData = hLines_raw[line]; + //˲˳쳣 + int ptNum = (int)lineData.size(); + sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], ptNum, filterParam); + + std::vector line_features; + int dataSize = (int)lineData.size(); + wd_simpleLineSegment( + lineData, + line, + lineLenRange, + lineSegPara, + line_features); + lineFeatures_h_raw.push_back(line_features); + } + + //ע + std::vector> featureInfoMask; + std::vector> feature3DInfo; + featureInfoMask.resize(lineNum); + feature3DInfo.resize(lineNum); + for (int i = 0; i < lineNum; i++) + { + featureInfoMask[i].resize(lineNum_h_raw); + feature3DInfo[i].resize(lineNum_h_raw); + } + + //ֱע + for (int line = 0; line < lineNum; line++) + { + if (line == 390) + int kkk = 1; + std::vector& a_lineJumpFeature = lineFeatures_v_raw[line]; + for (int m = 0, m_max = (int)a_lineJumpFeature.size(); m < m_max; m++) + { + int px = a_lineJumpFeature[m].lineIdx; + int py = a_lineJumpFeature[m].midPtIdx; + SSG_featureClusteringInfo& a_featureInfo = featureInfoMask[px][py]; + a_featureInfo.clusterID = 0; + a_featureInfo.featurType = 1; + a_featureInfo.featureIdx_v = m; + a_featureInfo.featureIdx_h = 0; + a_featureInfo.lineIdx = px; + a_featureInfo.ptIdx = py; + a_featureInfo.flag = 0; + SVzNL3DPoint& a_feature3D = feature3DInfo[px][py]; + a_feature3D = a_lineJumpFeature[m].midPt; + } + } + //ˮƽע + for (int line = 0; line < lineNum_h_raw; line++) + { + std::vector& a_lineJumpFeature = lineFeatures_h_raw[line]; + for (int m = 0, m_max = (int)a_lineJumpFeature.size(); m < m_max; m++) + { + int py = a_lineJumpFeature[m].lineIdx; + int px = a_lineJumpFeature[m].midPtIdx; + SSG_featureClusteringInfo& a_featureInfo = featureInfoMask[px][py]; + if (a_featureInfo.featurType == 0) + { + a_featureInfo.clusterID = 0; + a_featureInfo.lineIdx = px; + a_featureInfo.ptIdx = py; + a_featureInfo.flag = 0; + } + a_featureInfo.featurType += 2; + a_featureInfo.featureIdx_h = m; + SVzNL3DPoint& a_feature3DValue = feature3DInfo[px][py]; + a_feature3DValue = { a_lineJumpFeature[m].midPt.y, a_lineJumpFeature[m].midPt.x, a_lineJumpFeature[m].midPt.z }; + } + } + // + //õ˼룬ع˼·иЧ + std::vector> clusters; //ֻ¼λ + std::vector clustersRoi3D; + int clusterID = 1; + int clusterCheckWin = 5; + for (int y = 0; y < lineNum_h_raw; y++) + { + for (int x = 0; x < lineNum; x++) + { + SSG_featureClusteringInfo& a_featureInfo = featureInfoMask[x][y]; + if ( (0 == a_featureInfo.featurType) || (a_featureInfo.clusterID > 0)) //Ѿ + continue; + + SVzNL3DPoint& a_feature3DValue = feature3DInfo[x][y]; + SVzNL3DRangeD a_clusterRoi; + a_clusterRoi.xRange.min = a_feature3DValue.x; + a_clusterRoi.xRange.max = a_feature3DValue.x; + a_clusterRoi.yRange.min = a_feature3DValue.y; + a_clusterRoi.yRange.max = a_feature3DValue.y; + a_clusterRoi.zRange.min = a_feature3DValue.z; + a_clusterRoi.zRange.max = a_feature3DValue.z; + + SVzNL2DPoint a_seedPos = {x, y}; + std::vector< SVzNL2DPoint> a_cluster; + a_cluster.push_back(a_seedPos); + wd_pointClustering2D( + featureInfoMask,//int¼ǺclusterIDһflag + feature3DInfo,//double,¼Ϣ + clusterCheckWin, // + growParam,// + clusterID, //ǰClusterID + a_cluster, //result + a_clusterRoi + ); + clusters.push_back(a_cluster); + clustersRoi3D.push_back(a_clusterRoi); + clusterID++; + } + } + // + //ȡǰһ + int clusterSize = (int)clusters.size(); + int bestClusterIdx = -1; + double minZ = -1; + double wTh1 = gasFillingPortPara.innerD * 0.8; + double wTh2 = gasFillingPortPara.outerD * 1.1; + for (int i = 0; i < clusterSize; i++) + { + SVzNL3DRangeD& a_roi = clustersRoi3D[i]; + double xWidth = a_roi.xRange.max - a_roi.xRange.min; + double yWidth = a_roi.yRange.max - a_roi.yRange.min; + double meanZ = (a_roi.zRange.min + a_roi.zRange.max) / 2; + if ((xWidth > wTh1) && (xWidth < wTh2) && (yWidth > wTh1) && (yWidth < wTh2)) + { + if (minZ < 0) + { + minZ = meanZ; + bestClusterIdx = i; + } + else + { + if (minZ > meanZ) + { + minZ = meanZ; + bestClusterIdx = i; + } + } + } + } + if (minZ < 0) + { + *errCode = SG_ERR_NOT_GRID_FORMAT; + return resultPose; + } + + std::vector< SVzNL2DPoint>& obj_cluster = clusters[bestClusterIdx]; + //ȡϵĵ + std::vector< cv::Point3f> planePts; + for (int i = 0, i_max = (int)obj_cluster.size(); i < i_max; i++) + { + SVzNL2DPoint a_pos = obj_cluster[i]; + SSG_featureClusteringInfo& a_featureInfo = featureInfoMask[a_pos.x][a_pos.y]; + int type_v = a_featureInfo.featurType & 0x01; + int type_h = a_featureInfo.featurType & 0x02; + if (type_v > 0) + { + int lineIdx = a_featureInfo.lineIdx; + int featureIdx = a_featureInfo.featureIdx_v; + SSG_featureSemiCircle& a_feature = lineFeatures_v_raw[lineIdx][featureIdx]; + for (int m = a_feature.startPtIdx; m <= a_feature.endPtIdx; m++) + { + SVzNL3DPosition& a_pt3d = scanLines[lineIdx][m]; + if ((a_pt3d.nPointIdx == 0) && (a_pt3d.pt3D.z > 1e-4)) + { + a_pt3d.nPointIdx = 1; + cv::Point3f a_planePt = { (float)a_pt3d.pt3D.x, (float)a_pt3d.pt3D.y, (float)a_pt3d.pt3D.z }; + planePts.push_back(a_planePt); + } + } + } + if (type_h > 0) + { + int ptIdx = a_featureInfo.ptIdx; + int featureIdx = a_featureInfo.featureIdx_h; + SSG_featureSemiCircle& a_feature = lineFeatures_h_raw[ptIdx][featureIdx]; + for (int m = a_feature.startPtIdx; m <= a_feature.endPtIdx; m++) + { + SVzNL3DPosition& a_pt3d = scanLines[m][ptIdx]; + if ( (a_pt3d.nPointIdx == 0) && (a_pt3d.pt3D.z >1e-4)) + { + a_pt3d.nPointIdx = 1; + cv::Point3f a_planePt = { (float)a_pt3d.pt3D.x, (float)a_pt3d.pt3D.y, (float)a_pt3d.pt3D.z }; + planePts.push_back(a_planePt); + } + } + } + } + // ƽ棬㷨 + //: z = Ax + By + C + //res: [0]=A, [1]= B, [2]=-1.0, [3]=C, + std::vector res; + vzCaculateLaserPlane(planePts, res); + //ƽΪˮƽƽ + SSG_planeCalibPara calibPara = adjustPlaneToXYPlane( + res[0], res[1], res[2] //ƽ淨 + ); + + //ͶӰ + std::vector< SVzNL3DPoint> projectionPts; + double calibMeanZ = 0; + int planePtSize = (int)planePts.size(); + for(int i = 0; i < planePtSize; i ++) + { + SVzNL3DPoint a_calibPt; + a_calibPt.x = (float)(planePts[i].x * calibPara.planeCalib[0] + planePts[i].y * calibPara.planeCalib[1] + planePts[i].z * calibPara.planeCalib[2]); + a_calibPt.y = (float)(planePts[i].x * calibPara.planeCalib[3] + planePts[i].y * calibPara.planeCalib[4] + planePts[i].z * calibPara.planeCalib[5]); + a_calibPt.z = (float)(planePts[i].x * calibPara.planeCalib[6] + planePts[i].y * calibPara.planeCalib[7] + planePts[i].z * calibPara.planeCalib[8]); + calibMeanZ += a_calibPt.z; + projectionPts.push_back(a_calibPt); + } + calibMeanZ = calibMeanZ / planePtSize; + //ԲС + SVzNL3DPoint calibCenter; + double radius = 0; + double fittingErr = fitCircleByLeastSquare( + projectionPts, + calibCenter, + radius); + calibCenter.z = calibMeanZ; + + //centerתȥ + SVzNL3DPoint center; + center.x = (float)(calibCenter.x * calibPara.invRMatrix[0] + calibCenter.y * calibPara.invRMatrix[1] + calibCenter.z * calibPara.invRMatrix[2]); + center.y = (float)(calibCenter.x * calibPara.invRMatrix[3] + calibCenter.y * calibPara.invRMatrix[4] + calibCenter.z * calibPara.invRMatrix[5]); + center.z = (float)(calibCenter.x * calibPara.invRMatrix[6] + calibCenter.y * calibPara.invRMatrix[7] + calibCenter.z * calibPara.invRMatrix[8]); + + resultPose.x = center.x; + resultPose.y = center.y; + resultPose.z = center.z; + resultPose.x_roll = res[0]; + resultPose.y_pitch = res[1]; + resultPose.z_yaw = res[2]; + + return resultPose; + +} \ No newline at end of file diff --git a/sourceCode/gasFillingPortPosition_Export.h b/sourceCode/gasFillingPortPosition_Export.h new file mode 100644 index 0000000..9f001a7 --- /dev/null +++ b/sourceCode/gasFillingPortPosition_Export.h @@ -0,0 +1,25 @@ +#pragma once + +#include "SG_algo_Export.h" +#include + +#define _OUTPUT_DEBUG_DATA 1 + +typedef struct +{ + double innerD; //ڶھ42mm + double outerD; //ڶ⾶52mm +}SSX_gasFillingPortPara; + +//汾 +SG_APISHARED_EXPORT const char* wd_gasFillingPortPositionVersion(void); + +//ȡλ +SG_APISHARED_EXPORT SSG_6DOF wd_getGasFillingPortPosition( + std::vector< std::vector>& scanLines, + const SSX_gasFillingPortPara gasFillingPortPara, + const SSG_lineSegParam lineSegPara, + const SSG_outlierFilterParam filterParam, + SSG_treeGrowParam growParam, + int* errCode); +