diff --git a/BQ_assemblyPosition/BQ_assemblyPosition.vcxproj b/BQ_assemblyPosition/BQ_assemblyPosition.vcxproj new file mode 100644 index 0000000..fee28df --- /dev/null +++ b/BQ_assemblyPosition/BQ_assemblyPosition.vcxproj @@ -0,0 +1,172 @@ + + + + + Debug + Win32 + + + Release + Win32 + + + Debug + x64 + + + Release + x64 + + + + + + + + + + 16.0 + Win32Proj + {0e62eee4-abb5-4364-a794-ccffe8815426} + BQassemblyPosition + 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;BQASSEMBLYPOSITION_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) + true + Use + pch.h + + + Windows + true + false + + + + + Level3 + true + true + true + WIN32;NDEBUG;BQASSEMBLYPOSITION_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) + true + Use + pch.h + + + Windows + true + true + true + false + + + + + Level3 + true + _CRT_SECURE_NO_WARNINGS;_DEBUG;BQASSEMBLYPOSITION_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;BQASSEMBLYPOSITION_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/BQ_assemblyPosition_test/BQ_assemblyPosition_test.cpp b/BQ_assemblyPosition_test/BQ_assemblyPosition_test.cpp new file mode 100644 index 0000000..b5a7a67 --- /dev/null +++ b/BQ_assemblyPosition_test/BQ_assemblyPosition_test.cpp @@ -0,0 +1,20 @@ +// BQ_assemblyPosition_test.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。 +// + +#include + +int main() +{ + std::cout << "Hello World!\n"; +} + +// 运行程序: Ctrl + F5 或调试 >“开始执行(不调试)”菜单 +// 调试程序: F5 或调试 >“开始调试”菜单 + +// 入门使用技巧: +// 1. 使用解决方案资源管理器窗口添加/管理文件 +// 2. 使用团队资源管理器窗口连接到源代码管理 +// 3. 使用输出窗口查看生成输出和其他消息 +// 4. 使用错误列表窗口查看错误 +// 5. 转到“项目”>“添加新项”以创建新的代码文件，或转到“项目”>“添加现有项”以将现有代码文件添加到项目 +// 6. 将来，若要再次打开此项目，请转到“文件”>“打开”>“项目”并选择 .sln 文件 diff --git a/BQ_assemblyPosition_test/BQ_assemblyPosition_test.vcxproj b/BQ_assemblyPosition_test/BQ_assemblyPosition_test.vcxproj new file mode 100644 index 0000000..d84f2c0 --- /dev/null +++ b/BQ_assemblyPosition_test/BQ_assemblyPosition_test.vcxproj @@ -0,0 +1,147 @@ + + + + + Debug + Win32 + + + Release + Win32 + + + Debug + x64 + + + Release + x64 + + + + 16.0 + Win32Proj + {bc38d1e5-10cb-438b-ac72-6012303ce139} + BQassemblyPositiontest + 10.0 + + + + Application + true + v142 + Unicode + + + Application + false + v142 + true + Unicode + + + Application + true + v142 + Unicode + + + Application + false + v142 + true + Unicode + + + + + + + + + + + + + + + + + + + + + true + + + false + + + true + + + false + + + + Level3 + true + WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + + + Console + true + + + + + Level3 + true + true + true + WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + + + Console + true + true + true + + + + + Level3 + true + _DEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + + + Console + true + + + + + Level3 + true + true + true + NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + + + Console + true + true + true + + + + + + + + + \ No newline at end of file diff --git a/SG_Algorithm.sln b/SG_Algorithm.sln index 85ce659..c32da6c 100644 --- a/SG_Algorithm.sln +++ b/SG_Algorithm.sln @@ -151,6 +151,9 @@ Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "gasFillingPortPosition_test EndProjectSection EndProject Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "BQ_assemblyPosition", "BQ_assemblyPosition\BQ_assemblyPosition.vcxproj", "{0E62EEE4-ABB5-4364-A794-CCFFE8815426}" + ProjectSection(ProjectDependencies) = postProject + {95DC3F1A-902A-490E-BD3B-B10463CF0EBD} = {95DC3F1A-902A-490E-BD3B-B10463CF0EBD} + EndProjectSection EndProject Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "BQ_assemblyPosition_test", "BQ_assemblyPosition_test\BQ_assemblyPosition_test.vcxproj", "{BC38D1E5-10CB-438B-AC72-6012303CE139}" EndProject diff --git a/baseAlgorithm/baseAlgorithm.vcxproj b/baseAlgorithm/baseAlgorithm.vcxproj index 21bd19d..157bced 100644 --- a/baseAlgorithm/baseAlgorithm.vcxproj +++ b/baseAlgorithm/baseAlgorithm.vcxproj @@ -78,12 +78,12 @@ true - ..\..\thirdParty\VzNLSDK\Inc;..\..\thirdParty\opencv320\build\include;..\sourceCode;..\sourceCode\inc;$(IncludePath) + ..\..\thirdParty\VzNLSDK\Inc;..\..\thirdParty\opencv320\build\include;..\sourceCode;..\sourceCode\inc;F:\ShangGu\ProductDev\AlgoDev\thirdParty\PCL 1.12.0\3rdParty\Eigen\eigen3;$(IncludePath) $(SolutionDir)build\$(Platform)\$(Configuration)\ false - ..\..\thirdParty\VzNLSDK\Inc;..\..\thirdParty\opencv320\build\include;..\sourceCode;..\sourceCode\inc;$(IncludePath) + ..\..\thirdParty\VzNLSDK\Inc;..\..\thirdParty\opencv320\build\include;..\sourceCode;..\sourceCode\inc;F:\ShangGu\ProductDev\AlgoDev\thirdParty\PCL 1.12.0\3rdParty\Eigen\eigen3;$(IncludePath) $(SolutionDir)build\$(Platform)\$(Configuration)\ diff --git a/sourceCode/BQ_assemblyPosition.cpp b/sourceCode/BQ_assemblyPosition.cpp new file mode 100644 index 0000000..81407a0 --- /dev/null +++ b/sourceCode/BQ_assemblyPosition.cpp @@ -0,0 +1,442 @@ +#include +#include "SG_baseDataType.h" +#include "SG_baseAlgo_Export.h" +#include "BQ_assemblyPosition_Export.h" +#include +#include + +//version 1.0.0 : base version release to customer +std::string m_strVersion = "1.0.0"; +const char* wd_BQAssemblyPositionVersion(void) +{ + return m_strVersion.c_str(); +} + +//һƽƽ +//пһƽͲοƽƽ棬ߵƽеƽ +//תΪƽƽ淨ΪֱIJ +SSG_planeCalibPara sx_BQ_getBaseCalibPara( + std::vector< std::vector>& scanLines) +{ + return sg_getPlaneCalibPara2(scanLines); +} + +//̬ƽȥ +void sx_BQ_lineDataR( + std::vector< SVzNL3DPosition>& a_line, + const double* camPoseR, + double groundH) +{ + lineDataRT_vector(a_line, camPoseR, groundH); +} + +int _counterLinePtNum(std::vector& lineData) +{ + int ptNum = 0; + for (int i = 0, i_max = (int)lineData.size(); i < i_max; i++) + { + if ((abs(lineData[i].pt3D.z) > 1e-4) || + (abs(lineData[i].pt3D.x > 1e-4)) || + (abs(lineData[i].pt3D.y > 1e-4))) + ptNum++; + } + return ptNum; +} + +bool compareByAngle(const SWD_polarPt& a, const SWD_polarPt& b) { + return a.angle < b.angle; +} + +SSX_BQAssemblyInfo sx_BQ_computeAssemblyInfoFrom3D( + std::vector< std::vector>& scanLines, + const SSG_cornerParam cornerPara, + const SSG_outlierFilterParam filterParam, + SSG_treeGrowParam growParam, + SSG_planeCalibPara groundCalibPara, + SSX_BQAssemblyPara assemblyParam, + int* errCode) +{ + *errCode = 0; + SSX_BQAssemblyInfo assemblyPose; + memset(&assemblyPose, 0, sizeof(SSX_BQAssemblyInfo)); + int lineNum = (int)scanLines.size(); + if (lineNum == 0) + { + *errCode = SG_ERR_3D_DATA_NULL; + return assemblyPose; + } + + //ʼͽĿհɨȥɨ߽ + int validStartLine = -1; + for (int i = 0; i < lineNum; i++) + { + int linePtNum = _counterLinePtNum(scanLines[i]); + if (linePtNum > 0) + { + validStartLine = i; + break; + } + } + int validEndLine = -1; + for (int i = lineNum - 1; i >= 0; i--) + { + int linePtNum = _counterLinePtNum(scanLines[i]); + if (linePtNum > 0) + { + validEndLine = i; + break; + } + } + if ((validStartLine < 0) || (validEndLine < 0)) + { + *errCode = SG_ERR_3D_DATA_NULL; + return assemblyPose; + } + + int linePtNum = (int)scanLines[0].size(); + bool isGridData = true; + + //ӦתǶ + //ֱȡ + std::vector> jumpFeatures_v_raw; + for (int line = 0; line < lineNum; line++) + { + if (line == 250) + 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(); + sg_getLineCornerFeature_BQ( + &lineData[0], + dataSize, + line, + groundCalibPara.planeHeight, + cornerPara, //scaleͨȡbagH1/4 + line_features); + jumpFeatures_v_raw.push_back(line_features); + } + + if (false == isGridData)//ݲʽ + { + *errCode = SG_ERR_NOT_GRID_FORMAT; + return assemblyPose; + } + + //ˮƽɨ + 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> jumpFeatures_h_raw; + int lineNum_h_raw = (int)hLines_raw.size(); + for (int line = 0; line < lineNum_h_raw; line++) + { + if (line == 416) + 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(); + sg_getLineCornerFeature_BQ( + &hLines_raw[line][0], + dataSize, + line, + groundCalibPara.planeHeight, + cornerPara, //scaleͨȡbagH1/4 + line_features); + jumpFeatures_h_raw.push_back(line_features); + } + + //˳ + //ֱ߷ + std::vector v_trees; + for (int line = 0; line < lineNum; line++) + { + bool isLastLine = false; + if (line == lineNum - 1) + isLastLine = true; + std::vector& a_lineJumpFeature = jumpFeatures_v_raw[line]; + if (a_lineJumpFeature.size() > 0) + int kkk = 1; + if (line == 202) + int kkk = 1; + sg_lineFeaturesGrowing( + line, + isLastLine, + a_lineJumpFeature, + v_trees, + growParam); + } + + //ˮƽɨ˶ + std::vector h_trees; + for (int line = 0; line < lineNum_h_raw; line++) + { + if (line == 650) + int kkk = 1; + bool isLastLine = false; + if (line == lineNum_h_raw - 1) + isLastLine = true; + std::vector& a_lineJumpFeature = jumpFeatures_h_raw[line]; + sg_lineFeaturesGrowing( + line, + isLastLine, + a_lineJumpFeature, + h_trees, + growParam); + } + + std::vector polarPoints; + for (int i = 0, i_max = (int)v_trees.size(); i < i_max; i++) + { + SSG_featureTree* a_vTree = &v_trees[i]; + //ԭʼϱǣͬʱMaskϱ + for (int j = 0, j_max = (int)a_vTree->treeNodes.size(); j < j_max; j++) + { + int lineIdx = a_vTree->treeNodes[j].jumpPos2D.x; + int ptIdx = a_vTree->treeNodes[j].jumpPos2D.y; + if (scanLines[lineIdx][ptIdx].nPointIdx >= 0) + { + SWD_polarPt a_polarPt; + a_polarPt.lineIdx = lineIdx; + a_polarPt.ptIdx = ptIdx; + a_polarPt.R = 0; + a_polarPt.angle = 0; + a_polarPt.x = scanLines[lineIdx][ptIdx].pt3D.x; + a_polarPt.y = scanLines[lineIdx][ptIdx].pt3D.y; + a_polarPt.z = scanLines[lineIdx][ptIdx].pt3D.z; + polarPoints.push_back(a_polarPt); + scanLines[lineIdx][ptIdx].nPointIdx = -1; + } + } + } + for (int i = 0, i_max = (int)h_trees.size(); i < i_max; i++) + { + SSG_featureTree* a_hTree = &h_trees[i]; + //ԭʼϱǣͬʱMaskϱ + for (int j = 0, j_max = (int)a_hTree->treeNodes.size(); j < j_max; j++) + { + int lineIdx = a_hTree->treeNodes[j].jumpPos2D.y; + int ptIdx = a_hTree->treeNodes[j].jumpPos2D.x; + if (scanLines[lineIdx][ptIdx].nPointIdx >= 0) + { + SWD_polarPt a_polarPt; + a_polarPt.lineIdx = lineIdx; + a_polarPt.ptIdx = ptIdx; + a_polarPt.cptIndex = -1; + a_polarPt.R = 0; + a_polarPt.angle = 0; + a_polarPt.x = scanLines[lineIdx][ptIdx].pt3D.x; + a_polarPt.y = scanLines[lineIdx][ptIdx].pt3D.y; + a_polarPt.z = scanLines[lineIdx][ptIdx].pt3D.z; + polarPoints.push_back(a_polarPt); + scanLines[lineIdx][ptIdx].nPointIdx = -1; + } + } + } + + //㼸 + int contourPtSize = (int)polarPoints.size(); + if (contourPtSize == 0) + { + *errCode = SX_ERR_ZERO_CONTOUR_PT; + return assemblyPose; + } + double center_x = 0; + double center_y = 0; + for (int pi = 0; pi < contourPtSize; pi++) + { + center_x += polarPoints[pi].x; + center_y += polarPoints[pi].y; + } + center_x = center_x / (double)contourPtSize; + center_y = center_y / (double)contourPtSize; + //㼫RTheta + for (int pi = 0; pi < contourPtSize; pi++) + { + double angle = atan2(polarPoints[pi].y - center_y, polarPoints[pi].x - center_x); + angle = (angle / PI) * 180 + 180.0; + double R = sqrt(pow(polarPoints[pi].y - center_y, 2) + pow(polarPoints[pi].x - center_x, 2)); + polarPoints[pi].R = R; + polarPoints[pi].angle = angle; + } + //ǶȴС + std::sort(polarPoints.begin(), polarPoints.end(), compareByAngle); + for (int pi = 0; pi < contourPtSize; pi++) + polarPoints[pi].cptIndex = pi; // index + + //ȡRֵ + double minR = -1, maxR = -1; //СRûз֧minRmaxRСʱΪԲλ8Σûз֧ + int minRPos = -1; + std::vector polarRPeakPts; + int winSize = contourPtSize / 36; //+-10ȷΧ + if (winSize < 5) + winSize = 5; + for (int pi = 0; pi < contourPtSize; pi++) + { + double currR = polarPoints[pi].R; + if (minR < 0) + { + minR = currR; + maxR = currR; + minRPos = pi; + } + else + { + minRPos = minR > currR ? pi : minRPos; + minR = minR > currR ? currR : minR; + maxR = maxR < currR ? currR : maxR; + } + bool isPeak = true; + for (int k = -winSize; k <= winSize; k++) + { + int idx = (pi + k + contourPtSize) % contourPtSize; //Ͳνṹ + if (polarPoints[idx].R > currR) + { + isPeak = false; + break; + } + } + if (true == isPeak) + polarRPeakPts.push_back(polarPoints[pi]); + } + + double ratio_MaxMin = maxR / minR; + bool hasBranch = ratio_MaxMin < 1.25 ? false : true; + + std::vector validPolarRPeakPts; + std::vector polarPeakInfo; + int pkId = 0; + //ԲεļֵƫƣԲҲγɼֵݼֵL=ֱ߶γȹɵŽж + double arcAngleChkLen = 100; //ԲŽǵij߶ + for (int i = 0, i_max = (int)polarRPeakPts.size(); i < i_max; i++) + { + int ptidx = polarRPeakPts[i].cptIndex; + double px, py, pz; + px = polarRPeakPts[i].x; + py = polarRPeakPts[i].y; + + int LL1 = -1; + int halfLL1 = -1; + for (int j = ptidx - 1; j > -contourPtSize; j--) + { + int idx = (j + contourPtSize) % contourPtSize; //Ͳνṹ + double cx = polarPoints[idx].x; + double cy = polarPoints[idx].y; + double len = sqrt(pow(px - cx, 2) + pow(py - cy, 2)); + if (len < arcAngleChkLen) + halfLL1 = idx; + if (len > (assemblyParam.lineLen)) + { + LL1 = idx; + break; + } + } + + int LL2 = -1; + int halfLL2 = -1; + for (int j = ptidx + 1; j < contourPtSize * 2; j++) + { + int idx = j % contourPtSize; //Ͳνṹ + double cx = polarPoints[idx].x; + double cy = polarPoints[idx].y; + double len = sqrt(pow(px - cx, 2) + pow(py - cy, 2)); + if (len < arcAngleChkLen) + halfLL2 = idx; + if (len > (assemblyParam.lineLen)) + { + LL2 = idx; + break; + } + } + + if ((LL1 >= 0) && (LL2 >= 0)) + { + double len1 = sqrt(pow(px - polarPoints[halfLL1].x, 2) + pow(py - polarPoints[halfLL1].y, 2)); + double len2 = sqrt(pow(px - polarPoints[halfLL2].x, 2) + pow(py - polarPoints[halfLL2].y, 2)); + double len3 = sqrt(pow(polarPoints[halfLL1].x - polarPoints[halfLL2].x, 2) + + pow(polarPoints[halfLL1].y - polarPoints[halfLL2].y, 2)); + double cosTheta = (len1 * len1 + len2 * len2 - len3 * len3) / (2 * len1 * len2); + double theta = acos(cosTheta) * 180.0 / PI; + if (theta < 150) + { + SWD_polarPeakInfo a_pkInfo; + a_pkInfo.cptIndex = ptidx; + a_pkInfo.L1_ptIndex = LL1; + a_pkInfo.L2_ptIndex = LL2; + a_pkInfo.cornerAngle = theta; + a_pkInfo.cornerDir = 0; + polarRPeakPts[i].cptIndex = ptidx; + polarRPeakPts[i].pkId = pkId; + pkId++; + validPolarRPeakPts.push_back(polarRPeakPts[i]); + polarPeakInfo.push_back(a_pkInfo); + } + } + } + //ι + if (validPolarRPeakPts.size() != 4) + { + *errCode = SX_ERR_INVLID_RPEAK_NUM; + return assemblyPose; + } + + //ȡXᣬ̱YᣬΪZᣬĵΪO + std::vector sidePts_1; + std::vector sidePts_2; + std::vector sidePts_3; + std::vector sidePts_4; + return assemblyPose; +} + +//Mark㹤λú̬ +SSX_BQAssemblyInfo sx_BQ_computeAssemblyInfoFromMark( + SSX_BQAssemblyInfo originPos, + std::vector originMarkPos, + std::vector currMarkPos, + int* errCode) +{ + *errCode = 0; + SSX_BQAssemblyInfo resultPos; + memset(&resultPos, 0, sizeof(SSX_BQAssemblyInfo)); + if ((originMarkPos.size() < 3) || (currMarkPos.size() < 3)) + { + *errCode = SX_ERR_INVLID_MARK_NUM; + return resultPos; + } + + cv::Mat R, T; //תƽ + cv::Point3d C_origin, C_curr; // + caculateRT(originMarkPos, currMarkPos, R, T, C_origin, C_curr); + + // 5. Pn + pointRT(R, T, C_origin, C_curr, originPos.O, resultPos.O); + pointRT(R, T, C_origin, C_curr, originPos.X, resultPos.X); + pointRT(R, T, C_origin, C_curr, originPos.Y, resultPos.Y); + pointRT(R, T, C_origin, C_curr, originPos.Z, resultPos.Z); + return resultPos; +} + + + diff --git a/sourceCode/BQ_assemblyPosition_Export.h b/sourceCode/BQ_assemblyPosition_Export.h new file mode 100644 index 0000000..d431473 --- /dev/null +++ b/sourceCode/BQ_assemblyPosition_Export.h @@ -0,0 +1,49 @@ +#pragma once + +#include "SG_algo_Export.h" +#include + +#define _OUTPUT_DEBUG_DATA 1 + +typedef struct +{ + int type; // + double lineLen; //ֱ߶γ +}SSX_BQAssemblyPara; + +typedef struct +{ + int type; // + cv::Point3d O; //ĵ + cv::Point3d X; //ϵOX + cv::Point3d Y; //ϵOY + cv::Point3d Z; //ϵOZ +}SSX_BQAssemblyInfo; + +//汾 +SG_APISHARED_EXPORT const char* wd_BQAssemblyPositionVersion(void); + +//һƽƽ +//пһƽͲοƽƽ棬ߵƽеƽ +//תΪƽƽ淨ΪֱIJ +SG_APISHARED_EXPORT SSG_planeCalibPara sx_BQ_getBaseCalibPara( + std::vector< std::vector>& scanLines); + +//̬ƽȥ +SG_APISHARED_EXPORT void sx_BQ_lineDataR( + std::vector< SVzNL3DPosition>& a_line, + const double* camPoseR, + double groundH); + +//ȡǵ㼰λϢ +SG_APISHARED_EXPORT SSX_BQAssemblyInfo sx_BQ_getAssemblyInfo( + std::vector< std::vector>& scanLines, + const SSG_cornerParam cornerPara, + const SSG_outlierFilterParam filterParam, + SSG_treeGrowParam growParam, + SSG_planeCalibPara groundCalibPara, + SSX_BQAssemblyPara assemblyParam, + int* errCode); + + + diff --git a/sourceCode/BQ_workpieceCornerExtraction.cpp b/sourceCode/BQ_workpieceCornerExtraction.cpp index f9512c8..6c8dbc7 100644 --- a/sourceCode/BQ_workpieceCornerExtraction.cpp +++ b/sourceCode/BQ_workpieceCornerExtraction.cpp @@ -163,29 +163,6 @@ typedef struct SVzNL3DPoint edge_link2_ends[2]; }SSX_featureContour; -typedef struct -{ - int pkId; - int lineIdx; - int ptIdx; - int cptIndex; //Բɨϵĵ - //double cornerAngle; //ԵΪĵҵļн - double R; - double angle; - double x; - double y; - double z; -}SWD_polarPt; - -typedef struct -{ - int cptIndex; - int L1_ptIndex; - int L2_ptIndex; - double cornerAngle; - int cornerDir; -}SWD_polarPeakInfo; - typedef struct { double angle; diff --git a/sourceCode/SG_baseAlgo_Export.h b/sourceCode/SG_baseAlgo_Export.h index 7fab3a8..109ed8b 100644 --- a/sourceCode/SG_baseAlgo_Export.h +++ b/sourceCode/SG_baseAlgo_Export.h @@ -538,4 +538,17 @@ SG_APISHARED_EXPORT void wd_seedWatershed( SG_APISHARED_EXPORT void wd_noiseFilter( std::vector< std::vector>& scanLines, const SSG_outlierFilterParam filterParam, - int* errCode); \ No newline at end of file + int* errCode); + +// Eigenʵ֣pts1pts2תƽƾ +//Ҫ +SG_APISHARED_EXPORT void caculateRT( + const std::vector& pts1, + const std::vector& pts2, + cv::Mat& R, cv::Mat& T, + cv::Point3d& C1, cv::Point3d& C2); + +//תƽƺλ +SG_APISHARED_EXPORT void pointRT(const cv::Mat& R, const cv::Mat& T, + const cv::Point3d& originPt, const cv::Point3d& rtOriginPT, //RT(תƽƣǰ + const cv::Point3d& pt, cv::Point3d& rtPt); //RTǰĵ diff --git a/sourceCode/SG_baseDataType.h b/sourceCode/SG_baseDataType.h index 6c05012..b7dec1f 100644 --- a/sourceCode/SG_baseDataType.h +++ b/sourceCode/SG_baseDataType.h @@ -501,4 +501,27 @@ typedef struct { SVzNL3DPoint pt1; SVzNL3DPoint pt2; -}SWD_3DPointPair; \ No newline at end of file +}SWD_3DPointPair; + +typedef struct +{ + int pkId; + int lineIdx; + int ptIdx; + int cptIndex; //Բɨϵĵ + //double cornerAngle; //ԵΪĵҵļн + double R; + double angle; + double x; + double y; + double z; +}SWD_polarPt; + +typedef struct +{ + int cptIndex; + int L1_ptIndex; + int L2_ptIndex; + double cornerAngle; + int cornerDir; +}SWD_polarPeakInfo; diff --git a/sourceCode/SG_baseFunc.cpp b/sourceCode/SG_baseFunc.cpp index 1a61b98..8cfbee9 100644 --- a/sourceCode/SG_baseFunc.cpp +++ b/sourceCode/SG_baseFunc.cpp @@ -7,6 +7,7 @@ #include #include +#include SVzNL3DRangeD sg_getScanDataROI( //ɨROI @@ -2426,3 +2427,86 @@ void pointClout2DProjection( } } } + +//ԿռӦתƽƾ +// Eigenʵ +void caculateRT( + const std::vector& pts1, + const std::vector& pts2, + cv::Mat& R, cv::Mat& T, + cv::Point3d& C1, cv::Point3d& C2) +{ + //1 ĵ + cv::Point3d p1, p2; + int N = pts1.size(); + for (int i = 0; i < N; i++) + { + p1 += pts1[i]; + p2 += pts2[i]; + } + p1 = cv::Point3d(cv::Vec3d(p1) / N); + p2 = cv::Point3d(cv::Vec3d(p2) / N); + C1 = p1; + C2 = p2; + // 2õȥ + std::vector q1(N), q2(N); + for (int i = 0; i < N; i++) + { + q1[i] = pts1[i] - p1; + q2[i] = pts2[i] - p2; + } + + //3Ҫֵֽ W = sum(qi * qiת) compute q1*q2^T + Eigen::Matrix3d W = Eigen::Matrix3d::Zero(); + for (int i = 0; i < N; i++) + W += Eigen::Vector3d(q1[i].x, q1[i].y, q1[i].z) * Eigen::Vector3d(q2[i].x, q2[i].y, q2[i].z).transpose(); + + // 4 W SVD ֵֽ + Eigen::JacobiSVD svd(W, Eigen::ComputeFullU | Eigen::ComputeFullV); + Eigen::Matrix3d U = svd.matrixU(); + Eigen::Matrix3d V = svd.matrixV(); + + // 5ת ƽƾ R t, R= V *M* UT + double det = (U * V.transpose()).determinant(); + Eigen::Matrix3d M; + M << 1, 0, 0, 0, 1, 0, 0, 0, det; + + Eigen::Matrix3d R_ = V * M * (U.transpose()); + + // t = p' - R * p + Eigen::Vector3d t_ = Eigen::Vector3d(p2.x, p2.y, p2.z) - R_ * Eigen::Vector3d(p1.x, p1.y, p1.z); + + // 6ʽת convert to cv::Mat + R = (cv::Mat_(3, 3) << + R_(0, 0), R_(0, 1), R_(0, 2), + R_(1, 0), R_(1, 1), R_(1, 2), + R_(2, 0), R_(2, 1), R_(2, 2) + ); + + T = (cv::Mat_(3, 1) << t_(0, 0), t_(1, 0), t_(2, 0)); + return; +} + +//תƽƺλ +void pointRT(const cv::Mat& R, const cv::Mat& T, + const cv::Point3d& originPt, const cv::Point3d& rtOriginPT, //RT(תƽƣǰ + const cv::Point3d& pt, cv::Point3d& rtPt) //RTǰĵ +{ + Eigen::Matrix3d _R; + for (int i = 0; i < 3; ++i) { + for (int j = 0; j < 3; ++j) { + _R(i, j) = R.at(i, j); + } + } + Eigen::Vector3d _T = Eigen::Vector3d(T.at(0, 0), T.at(1, 0), T.at(2, 0)); + + Eigen::Vector3d vec_origin = Eigen::Vector3d(originPt.x, originPt.y, originPt.z); + Eigen::Vector3d vec_rtOrigin = Eigen::Vector3d(rtOriginPT.x, rtOriginPT.y, rtOriginPT.z); + Eigen::Vector3d vec_pt = Eigen::Vector3d(pt.x, pt.y, pt.z); + Eigen::Vector3d result = _R * (vec_pt - vec_origin) + vec_rtOrigin; + + rtPt.x = result(0); + rtPt.x = result(1); + rtPt.x = result(2); + return; +} diff --git a/sourceCode/SG_errCode.h b/sourceCode/SG_errCode.h index 0d2767c..1266374 100644 --- a/sourceCode/SG_errCode.h +++ b/sourceCode/SG_errCode.h @@ -16,6 +16,7 @@ #define SX_ERR_ZERO_CONTOUR_PT -2005 #define SX_ERR_INVLID_RPEAK_NUM -2006 #define SX_ERR_INVLID_RPEAK_PAIR -2007 +#define SX_ERR_INVLID_MARK_NUM -2008 //ץȡ #define SX_ERR_INVLID_CUTTING_Z -2101 @@ -23,3 +24,4 @@ // #define SX_BAG_TRAY_EMPTY -2201 + diff --git a/sourceCode/WD_bottleCapQualityCheck.cpp b/sourceCode/WD_bottleCapQualityCheck.cpp new file mode 100644 index 0000000..fa07438 --- /dev/null +++ b/sourceCode/WD_bottleCapQualityCheck.cpp @@ -0,0 +1,12 @@ +#include +#include "SG_baseDataType.h" +#include "SG_baseAlgo_Export.h" +#include "WD_bottleCapQualityCheck_Export.h" +#include +#include + +void wd_getBottleCapQualtiy( + std::vector< std::vector>& scanLines) +{ + return; +} \ No newline at end of file diff --git a/sourceCode/WD_bottleCapQualityCheck_Export.h b/sourceCode/WD_bottleCapQualityCheck_Export.h new file mode 100644 index 0000000..fa598a1 --- /dev/null +++ b/sourceCode/WD_bottleCapQualityCheck_Export.h @@ -0,0 +1,14 @@ +#pragma once + +#if defined(SG_API_LIBRARY) +# define SG_APISHARED_EXPORT __declspec(dllexport) +#else +# define SG_APISHARED_EXPORT __declspec(dllimport) +#endif + +#include "SG_baseDataType.h" +#include +#include + +SG_APISHARED_EXPORT void wd_getBottleCapQualtiy( + std::vector< std::vector>& scanLines);