workpieceHolePositioning version 1.6.4 : 拓普发5种工件添加正反判断。
This commit is contained in:
parent
3cbb9ce640
commit
e09a59bbd9
@ -249,6 +249,17 @@ Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "HC_planeLocalization_test",
|
||||
{95DC3F1A-902A-490E-BD3B-B10463CF0EBD} = {95DC3F1A-902A-490E-BD3B-B10463CF0EBD}
|
||||
EndProjectSection
|
||||
EndProject
|
||||
Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "hybridPosePositioning_test", "hybridPosePositioning_test\hybridPosePositioning_test.vcxproj", "{288E402C-183A-49AD-8EE1-5B1AE4E6F617}"
|
||||
ProjectSection(ProjectDependencies) = postProject
|
||||
{4B6EDA25-FCED-4AE3-BA12-AA559D8B71DC} = {4B6EDA25-FCED-4AE3-BA12-AA559D8B71DC}
|
||||
{95DC3F1A-902A-490E-BD3B-B10463CF0EBD} = {95DC3F1A-902A-490E-BD3B-B10463CF0EBD}
|
||||
EndProjectSection
|
||||
EndProject
|
||||
Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "hybridPosePositioning", "hybridPosePositioning\hybridPosePositioning.vcxproj", "{4B6EDA25-FCED-4AE3-BA12-AA559D8B71DC}"
|
||||
ProjectSection(ProjectDependencies) = postProject
|
||||
{95DC3F1A-902A-490E-BD3B-B10463CF0EBD} = {95DC3F1A-902A-490E-BD3B-B10463CF0EBD}
|
||||
EndProjectSection
|
||||
EndProject
|
||||
Global
|
||||
GlobalSection(SolutionConfigurationPlatforms) = preSolution
|
||||
Debug|x64 = Debug|x64
|
||||
@ -633,6 +644,22 @@ Global
|
||||
{E59226FE-E1FB-4F3B-9A93-A4173BE316B8}.Release|x64.Build.0 = Release|x64
|
||||
{E59226FE-E1FB-4F3B-9A93-A4173BE316B8}.Release|x86.ActiveCfg = Release|Win32
|
||||
{E59226FE-E1FB-4F3B-9A93-A4173BE316B8}.Release|x86.Build.0 = Release|Win32
|
||||
{288E402C-183A-49AD-8EE1-5B1AE4E6F617}.Debug|x64.ActiveCfg = Debug|x64
|
||||
{288E402C-183A-49AD-8EE1-5B1AE4E6F617}.Debug|x64.Build.0 = Debug|x64
|
||||
{288E402C-183A-49AD-8EE1-5B1AE4E6F617}.Debug|x86.ActiveCfg = Debug|Win32
|
||||
{288E402C-183A-49AD-8EE1-5B1AE4E6F617}.Debug|x86.Build.0 = Debug|Win32
|
||||
{288E402C-183A-49AD-8EE1-5B1AE4E6F617}.Release|x64.ActiveCfg = Release|x64
|
||||
{288E402C-183A-49AD-8EE1-5B1AE4E6F617}.Release|x64.Build.0 = Release|x64
|
||||
{288E402C-183A-49AD-8EE1-5B1AE4E6F617}.Release|x86.ActiveCfg = Release|Win32
|
||||
{288E402C-183A-49AD-8EE1-5B1AE4E6F617}.Release|x86.Build.0 = Release|Win32
|
||||
{4B6EDA25-FCED-4AE3-BA12-AA559D8B71DC}.Debug|x64.ActiveCfg = Debug|x64
|
||||
{4B6EDA25-FCED-4AE3-BA12-AA559D8B71DC}.Debug|x64.Build.0 = Debug|x64
|
||||
{4B6EDA25-FCED-4AE3-BA12-AA559D8B71DC}.Debug|x86.ActiveCfg = Debug|Win32
|
||||
{4B6EDA25-FCED-4AE3-BA12-AA559D8B71DC}.Debug|x86.Build.0 = Debug|Win32
|
||||
{4B6EDA25-FCED-4AE3-BA12-AA559D8B71DC}.Release|x64.ActiveCfg = Release|x64
|
||||
{4B6EDA25-FCED-4AE3-BA12-AA559D8B71DC}.Release|x64.Build.0 = Release|x64
|
||||
{4B6EDA25-FCED-4AE3-BA12-AA559D8B71DC}.Release|x86.ActiveCfg = Release|Win32
|
||||
{4B6EDA25-FCED-4AE3-BA12-AA559D8B71DC}.Release|x86.Build.0 = Release|Win32
|
||||
EndGlobalSection
|
||||
GlobalSection(SolutionProperties) = preSolution
|
||||
HideSolutionNode = FALSE
|
||||
|
||||
@ -12,6 +12,7 @@
|
||||
#define SG_ERR_SCAN_DIR_NOT_SUPPORTED -1009
|
||||
#define SX_ERR_ZERO_OBJECTS -1010
|
||||
#define SX_ERR_NULL_GROUND_PLANE -1011
|
||||
#define SX_ERR_ZERO_2D_OBJECTS -1012
|
||||
|
||||
//BQ_workpiece
|
||||
#define SX_ERR_INVLD_VTREE_NUM -2001
|
||||
|
||||
@ -33,7 +33,8 @@
|
||||
//version 1.6.1 : 添加了拓普发工件2、工件3、工件4的姿态计算。
|
||||
//version 1.6.2 : 拓普发算法代码同步。
|
||||
//version 1.6.3 : 拓普发5种工件算法。共3个API。 工件2和工作5使用API2, 工件3和工件4使用API3。
|
||||
std::string m_strVersion = "HolePostion 1.6.3";
|
||||
//version 1.6.4 : 拓普发5种工件添加正反判断。
|
||||
std::string m_strVersion = "HolePostion 1.6.4";
|
||||
const char* wd_workpieceHolePositioningVersion(void)
|
||||
{
|
||||
return m_strVersion.c_str();
|
||||
@ -2402,13 +2403,19 @@ void wd_workpieceHolePositioning_3(
|
||||
//内部参数
|
||||
double smallHoleMinDistance = 8.0;
|
||||
double maxDistanceFromCenterToLine = 25.0;
|
||||
double reverseJudgeDistToCenterTH = 33.0;
|
||||
double midPointToLongerHoleDIstTh = 20.0; //
|
||||
double distP1P2_short_minTh = 35.0;
|
||||
double distP1P2_short_maxTh = 48.0;
|
||||
double distP1P2_long_minTh = 25.0;
|
||||
double distP1P2_long_maxTh = 38.0;
|
||||
SVzNLRangeD distRange = { 0, bigHoleDiameter * 1.2 };
|
||||
double deltaZ = 10.0;
|
||||
std::vector< WD_workpieceInfo> allWorkpiece;
|
||||
double highest_z = -1;
|
||||
for (int objIdx = 0; objIdx < bigHoleSize; objIdx++)
|
||||
{
|
||||
if (objIdx == 42)
|
||||
if (objIdx == 11)
|
||||
int kkk = 1;
|
||||
|
||||
if (bigHoles[objIdx].radius < 0)
|
||||
@ -2425,10 +2432,23 @@ void wd_workpieceHolePositioning_3(
|
||||
if (groups.size() == 0) //工件3和工作4的特点是小孔分为上下各3个。分组
|
||||
continue;
|
||||
|
||||
std::vector<std::vector<double>> distancesToCenter;
|
||||
distancesToCenter.resize(groups.size());
|
||||
for (int grp = 0; grp < (int)groups.size(); grp++)
|
||||
{
|
||||
distancesToCenter[grp].resize(groups[grp].size());
|
||||
for (int m = 0; m < (int)groups[grp].size(); m++)
|
||||
{
|
||||
double dist = sqrt(pow(p0.center.x - groups[grp][m].center.x, 2) +
|
||||
pow(p0.center.y - groups[grp][m].center.y, 2));
|
||||
distancesToCenter[grp][m] = dist;
|
||||
}
|
||||
}
|
||||
//搜索每组中距离大于大孔直径0.8倍的目标
|
||||
SWD_HoleInfo p1, p2;
|
||||
bool foundObj = false;
|
||||
bool isYDIr = true;
|
||||
bool isReverse = false;
|
||||
for (int grp = 0; grp < (int)groups.size(); grp++)
|
||||
{
|
||||
std::vector<SWD_HoleInfo>& a_grp = groups[grp];
|
||||
@ -2457,6 +2477,23 @@ void wd_workpieceHolePositioning_3(
|
||||
p1 = a_grp[idx1];
|
||||
p2 = a_grp[idx2];
|
||||
foundObj = true;
|
||||
|
||||
double dist1 = distancesToCenter[grp][idx1];
|
||||
double dist2 = distancesToCenter[grp][idx2];
|
||||
if ((dist1 > reverseJudgeDistToCenterTH) && (dist2 > reverseJudgeDistToCenterTH)) //longer
|
||||
{
|
||||
if (p1.center.y < p0.center.y) //长的在上面，反
|
||||
isReverse = true;
|
||||
foundObj = true;
|
||||
break;
|
||||
}
|
||||
else if ((dist1 < reverseJudgeDistToCenterTH) && (dist2 < reverseJudgeDistToCenterTH)) //shorter
|
||||
{
|
||||
if (p1.center.y > p0.center.y) //短的在下面，反
|
||||
isReverse = true;
|
||||
foundObj = true;
|
||||
break;
|
||||
}
|
||||
break;
|
||||
}
|
||||
else if (validRelations.size() > 1)
|
||||
@ -2471,7 +2508,29 @@ void wd_workpieceHolePositioning_3(
|
||||
int idx2 = bestPair.data_1;
|
||||
p1 = a_grp[idx1];
|
||||
p2 = a_grp[idx2];
|
||||
foundObj = true;
|
||||
|
||||
double dist1 = distancesToCenter[grp][idx1];
|
||||
double dist2 = distancesToCenter[grp][idx2];
|
||||
if ((dist1 > reverseJudgeDistToCenterTH) && (dist2 > reverseJudgeDistToCenterTH)) //longer
|
||||
{
|
||||
if ((bestPair.value1 > distP1P2_long_minTh)&&(bestPair.value1 < distP1P2_long_maxTh))
|
||||
{
|
||||
if (p1.center.y < p0.center.y) //长的在上面，反
|
||||
isReverse = true;
|
||||
foundObj = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
else if ((dist1 < reverseJudgeDistToCenterTH) && (dist2 < reverseJudgeDistToCenterTH)) //shorter
|
||||
{
|
||||
if ( (bestPair.value1 > distP1P2_short_minTh)&& (bestPair.value1 < distP1P2_short_maxTh))
|
||||
{
|
||||
if (p1.center.y > p0.center.y) //短的在下面，反
|
||||
isReverse = true;
|
||||
foundObj = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
if (false == foundObj)
|
||||
@ -2513,6 +2572,39 @@ void wd_workpieceHolePositioning_3(
|
||||
p2 = pTmp2;
|
||||
foundObj = true;
|
||||
isYDIr = false;
|
||||
//判断正反
|
||||
bool judeOk = false;
|
||||
for (int n = 0; n < (int)groups[0].size(); n++)
|
||||
{
|
||||
double _dist = sqrt(pow(p1.center.x - groups[0][n].center.x, 2) +
|
||||
pow(p1.center.y - groups[0][n].center.y, 2));
|
||||
if ((distancesToCenter[0][n] > reverseJudgeDistToCenterTH) && (_dist < midPointToLongerHoleDIstTh))
|
||||
{
|
||||
if (groups[0][n].center.y < p0.center.y)
|
||||
{
|
||||
judeOk = true;
|
||||
isReverse = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
if (false == judeOk)
|
||||
{
|
||||
for (int n = 0; n < (int)groups[1].size(); n++)
|
||||
{
|
||||
double _dist = sqrt(pow(p2.center.x - groups[1][n].center.x, 2) +
|
||||
pow(p2.center.y - groups[1][n].center.y, 2));
|
||||
if ((distancesToCenter[1][n] > reverseJudgeDistToCenterTH) && (_dist < midPointToLongerHoleDIstTh))
|
||||
{
|
||||
if (groups[1][n].center.y < p0.center.y)
|
||||
{
|
||||
judeOk = true;
|
||||
isReverse = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -2521,7 +2613,6 @@ void wd_workpieceHolePositioning_3(
|
||||
continue;
|
||||
|
||||
SVzNL3DPoint centerPoint = p0.center;
|
||||
|
||||
//计算工件法向
|
||||
SVzNLRangeD distRange = { bigHoleDiameter / 2 + 2.0, bigHoleDiameter / 2 + 4.0 };
|
||||
std::vector<cv::Point3d> Points3ds;
|
||||
@ -2574,6 +2665,8 @@ void wd_workpieceHolePositioning_3(
|
||||
|
||||
WD_workpieceInfo a_workpiece;
|
||||
a_workpiece.workpieceType = smallHoleParam.workpieceType;
|
||||
if (true == isReverse) //反
|
||||
a_workpiece.workpieceType = a_workpiece.workpieceType | 0x10000;
|
||||
a_workpiece.center = centerPoint;
|
||||
y_dir = vec3_normalize(y_dir);
|
||||
a_workpiece.y_dir = { y_dir.x * 20 + a_workpiece.center.x, y_dir.y * 20 + a_workpiece.center.y, a_workpiece.center.z + y_dir.z * 20 };
|
||||
|
||||
@ -85,12 +85,12 @@
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
|
||||
<LinkIncremental>true</LinkIncremental>
|
||||
<OutDir>$(SolutionDir)build\$(Platform)\$(Configuration)\</OutDir>
|
||||
<IncludePath>..\..\thirdParty\VzNLSDK\Inc;..\..\thirdParty\opencv320\build\include;..\sourceCode;..\sourceCode\inc;$(IncludePath)</IncludePath>
|
||||
<IncludePath>..\..\thirdParty\VzNLSDK\Inc;..\sourceCode;..\sourceCode\inc;$(IncludePath)</IncludePath>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
|
||||
<LinkIncremental>false</LinkIncremental>
|
||||
<OutDir>$(SolutionDir)build\$(Platform)\$(Configuration)\</OutDir>
|
||||
<IncludePath>..\..\thirdParty\VzNLSDK\Inc;..\..\thirdParty\opencv320\build\include;..\sourceCode;..\sourceCode\inc;$(IncludePath)</IncludePath>
|
||||
<IncludePath>..\..\thirdParty\VzNLSDK\Inc;..\sourceCode;..\sourceCode\inc;$(IncludePath)</IncludePath>
|
||||
</PropertyGroup>
|
||||
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
|
||||
<ClCompile>
|
||||
|
||||
@ -189,7 +189,7 @@ void _outputWorkpieceInfo(char* fileName, std::vector< WD_workpieceInfo>& workpi
|
||||
int number = (int)workpiecePositioning.size();
|
||||
for (int i = 0; i < number; i++)
|
||||
{
|
||||
sprintf_s(dataStr, 250, "工件_%d", i + 1);
|
||||
sprintf_s(dataStr, 250, "工件_%d: type=%x", i + 1, workpiecePositioning[i].workpieceType);
|
||||
sw << dataStr << std::endl;
|
||||
int holeNumber = (int)workpiecePositioning[i].holes.size();
|
||||
for (int j = 0; j < holeNumber; j++)
|
||||
@ -947,11 +947,15 @@ void _XOYprojection(
|
||||
if (i == 0)
|
||||
{
|
||||
rgb = { 255, 0, 0 };
|
||||
if((holes[i].workpieceType&0xffff0000) > 0)//方向为反
|
||||
rgb = { 255, 0, 255 };
|
||||
size = int(3.0/scale);
|
||||
}
|
||||
else
|
||||
{
|
||||
rgb = { 255, 255, 0 };
|
||||
if ((holes[i].workpieceType & 0xffff0000) > 0)//方向为反
|
||||
rgb = { 0, 255, 255 };
|
||||
size = int(3.0 / scale);
|
||||
}
|
||||
WD_workpieceInfo& a_hole = holes[i];
|
||||
@ -1569,7 +1573,7 @@ void TuoPuFa_holePosition_test3(void)
|
||||
};
|
||||
|
||||
SVzNLRange fileIdx[TPF3_TEST_GROUP] = {
|
||||
{1,10}, {1, 10}, {1,10}, {1,10}, {1, 10},
|
||||
{1,11}, {1, 10}, {1,10}, {1,10}, {1, 10},
|
||||
{1,10}, {1, 10},
|
||||
};
|
||||
|
||||
@ -1635,7 +1639,7 @@ void TuoPuFa_holePosition_test3(void)
|
||||
|
||||
for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
|
||||
{
|
||||
//fidx = 2;
|
||||
//fidx = 3;
|
||||
char _scan_file[256];
|
||||
sprintf_s(_scan_file, "%s%d_LaserData_Jh26B074.txt", dataPath[grp], fidx);
|
||||
std::vector<std::vector< SVzNL3DPosition>> scanLines;
|
||||
@ -2736,11 +2740,11 @@ int main()
|
||||
TuoPuFa_holePosition_test5();
|
||||
else if (keSG_拓普发孔定位_工件全测试 == testMode)
|
||||
{
|
||||
TuoPuFa_holePosition_test();
|
||||
TuoPuFa_holePosition_test2();
|
||||
//TuoPuFa_holePosition_test();
|
||||
//TuoPuFa_holePosition_test2();
|
||||
TuoPuFa_holePosition_test3();
|
||||
TuoPuFa_holePosition_test4();
|
||||
TuoPuFa_holePosition_test5();
|
||||
//TuoPuFa_holePosition_test5();
|
||||
}
|
||||
else if (keSG_华航孔定位 == testMode)
|
||||
HuaHang_holePosition_test();
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user