sx_BQ_getWorkpieceCorners version 1.2.0
This commit is contained in:
parent
3621895832
commit
6c5adfc7d5
@ -232,59 +232,77 @@ void _outputCalibPara(char* fileName, SSG_planeCalibPara calibPara)
|
||||
void _outputCornerInfo(char* fileName, SSX_BQworkpieceResult workpieceCorner)
|
||||
{
|
||||
std::ofstream sw(fileName);
|
||||
|
||||
|
||||
sw << "节点" << workpieceCorner.workpieceType << std::endl << std::endl;
|
||||
char dataStr[250];
|
||||
sw << "L:" << std::endl;
|
||||
sprintf_s(dataStr, 250, " corner_0: (%g, %g, %g)", workpieceCorner.corner_L[0].x, workpieceCorner.corner_L[0].y, workpieceCorner.corner_L[0].z);
|
||||
sw << "A:" << std::endl;
|
||||
sprintf_s(dataStr, 250, " corner_0: (%g, %g, %g)", workpieceCorner.corner_1[0].x, workpieceCorner.corner_1[0].y, workpieceCorner.corner_1[0].z);
|
||||
sw << dataStr << std::endl;
|
||||
sprintf_s(dataStr, 250, " corner_1: (%g, %g, %g)", workpieceCorner.corner_L[1].x, workpieceCorner.corner_L[1].y, workpieceCorner.corner_L[1].z);
|
||||
sprintf_s(dataStr, 250, " corner_1: (%g, %g, %g)", workpieceCorner.corner_1[1].x, workpieceCorner.corner_1[1].y, workpieceCorner.corner_1[1].z);
|
||||
sw << dataStr << std::endl;
|
||||
sprintf_s(dataStr, 250, " corner_2: (%g, %g, %g)", workpieceCorner.corner_L[2].x, workpieceCorner.corner_L[2].y, workpieceCorner.corner_L[2].z);
|
||||
sprintf_s(dataStr, 250, " corner_2: (%g, %g, %g)", workpieceCorner.corner_1[2].x, workpieceCorner.corner_1[2].y, workpieceCorner.corner_1[2].z);
|
||||
sw << dataStr << std::endl;
|
||||
double dist = sqrt(pow(workpieceCorner.corner_L[0].x - workpieceCorner.corner_L[2].x, 2) +
|
||||
pow(workpieceCorner.corner_L[0].y - workpieceCorner.corner_L[2].y, 2) +
|
||||
pow(workpieceCorner.corner_L[0].z - workpieceCorner.corner_L[2].z, 2));
|
||||
sprintf_s(dataStr, 250, " Len: %g", dist);
|
||||
sw << dataStr << std::endl;
|
||||
|
||||
sw << "T:" << std::endl;
|
||||
sprintf_s(dataStr, 250, " corner_0: (%g, %g, %g)", workpieceCorner.corner_T[0].x, workpieceCorner.corner_T[0].y, workpieceCorner.corner_T[0].z);
|
||||
sw << dataStr << std::endl;
|
||||
sprintf_s(dataStr, 250, " corner_1: (%g, %g, %g)", workpieceCorner.corner_T[1].x, workpieceCorner.corner_T[1].y, workpieceCorner.corner_T[1].z);
|
||||
sw << dataStr << std::endl;
|
||||
sprintf_s(dataStr, 250, " corner_2: (%g, %g, %g)", workpieceCorner.corner_T[2].x, workpieceCorner.corner_T[2].y, workpieceCorner.corner_T[2].z);
|
||||
sw << dataStr << std::endl;
|
||||
dist = sqrt(pow(workpieceCorner.corner_T[0].x - workpieceCorner.corner_T[2].x, 2) +
|
||||
pow(workpieceCorner.corner_T[0].y - workpieceCorner.corner_T[2].y, 2) +
|
||||
pow(workpieceCorner.corner_T[0].z - workpieceCorner.corner_T[2].z, 2));
|
||||
sprintf_s(dataStr, 250, " Len: %g", dist);
|
||||
sw << dataStr << std::endl;
|
||||
|
||||
sw << "R:" << std::endl;
|
||||
sprintf_s(dataStr, 250, " corner_0: (%g, %g, %g)", workpieceCorner.corner_R[0].x, workpieceCorner.corner_R[0].y, workpieceCorner.corner_R[0].z);
|
||||
sw << dataStr << std::endl;
|
||||
sprintf_s(dataStr, 250, " corner_1: (%g, %g, %g)", workpieceCorner.corner_R[1].x, workpieceCorner.corner_R[1].y, workpieceCorner.corner_R[1].z);
|
||||
sw << dataStr << std::endl;
|
||||
sprintf_s(dataStr, 250, " corner_2: (%g, %g, %g)", workpieceCorner.corner_R[2].x, workpieceCorner.corner_R[2].y, workpieceCorner.corner_R[2].z);
|
||||
sw << dataStr << std::endl;
|
||||
dist = sqrt(pow(workpieceCorner.corner_R[0].x - workpieceCorner.corner_R[2].x, 2) +
|
||||
pow(workpieceCorner.corner_R[0].y - workpieceCorner.corner_R[2].y, 2) +
|
||||
pow(workpieceCorner.corner_R[0].z - workpieceCorner.corner_R[2].z, 2));
|
||||
double dist = sqrt(pow(workpieceCorner.corner_1[0].x - workpieceCorner.corner_1[2].x, 2) +
|
||||
pow(workpieceCorner.corner_1[0].y - workpieceCorner.corner_1[2].y, 2) +
|
||||
pow(workpieceCorner.corner_1[0].z - workpieceCorner.corner_1[2].z, 2));
|
||||
sprintf_s(dataStr, 250, " Len: %g", dist);
|
||||
sw << dataStr << std::endl;
|
||||
|
||||
sw << "B:" << std::endl;
|
||||
sprintf_s(dataStr, 250, " corner_0: (%g, %g, %g)", workpieceCorner.corner_B[0].x, workpieceCorner.corner_B[0].y, workpieceCorner.corner_B[0].z);
|
||||
sprintf_s(dataStr, 250, " corner_0: (%g, %g, %g)", workpieceCorner.corner_2[0].x, workpieceCorner.corner_2[0].y, workpieceCorner.corner_3[0].z);
|
||||
sw << dataStr << std::endl;
|
||||
sprintf_s(dataStr, 250, " corner_1: (%g, %g, %g)", workpieceCorner.corner_B[1].x, workpieceCorner.corner_B[1].y, workpieceCorner.corner_B[1].z);
|
||||
sprintf_s(dataStr, 250, " corner_1: (%g, %g, %g)", workpieceCorner.corner_2[1].x, workpieceCorner.corner_2[1].y, workpieceCorner.corner_3[1].z);
|
||||
sw << dataStr << std::endl;
|
||||
sprintf_s(dataStr, 250, " corner_2: (%g, %g, %g)", workpieceCorner.corner_B[2].x, workpieceCorner.corner_B[2].y, workpieceCorner.corner_B[2].z);
|
||||
sprintf_s(dataStr, 250, " corner_2: (%g, %g, %g)", workpieceCorner.corner_2[2].x, workpieceCorner.corner_2[2].y, workpieceCorner.corner_3[2].z);
|
||||
sw << dataStr << std::endl;
|
||||
dist = sqrt(pow(workpieceCorner.corner_B[0].x - workpieceCorner.corner_B[2].x, 2) +
|
||||
pow(workpieceCorner.corner_B[0].y - workpieceCorner.corner_B[2].y, 2) +
|
||||
pow(workpieceCorner.corner_B[0].z - workpieceCorner.corner_B[2].z, 2));
|
||||
dist = sqrt(pow(workpieceCorner.corner_2[0].x - workpieceCorner.corner_2[2].x, 2) +
|
||||
pow(workpieceCorner.corner_2[0].y - workpieceCorner.corner_2[2].y, 2) +
|
||||
pow(workpieceCorner.corner_2[0].z - workpieceCorner.corner_2[2].z, 2));
|
||||
sprintf_s(dataStr, 250, " Len: %g", dist);
|
||||
sw << dataStr << std::endl;
|
||||
|
||||
if (workpieceCorner.workpieceType != 3)
|
||||
{
|
||||
sw << "C:" << std::endl;
|
||||
sprintf_s(dataStr, 250, " corner_0: (%g, %g, %g)", workpieceCorner.corner_3[0].x, workpieceCorner.corner_3[0].y, workpieceCorner.corner_3[0].z);
|
||||
sw << dataStr << std::endl;
|
||||
sprintf_s(dataStr, 250, " corner_1: (%g, %g, %g)", workpieceCorner.corner_3[1].x, workpieceCorner.corner_3[1].y, workpieceCorner.corner_3[1].z);
|
||||
sw << dataStr << std::endl;
|
||||
sprintf_s(dataStr, 250, " corner_2: (%g, %g, %g)", workpieceCorner.corner_3[2].x, workpieceCorner.corner_3[2].y, workpieceCorner.corner_3[2].z);
|
||||
sw << dataStr << std::endl;
|
||||
dist = sqrt(pow(workpieceCorner.corner_3[0].x - workpieceCorner.corner_3[2].x, 2) +
|
||||
pow(workpieceCorner.corner_3[0].y - workpieceCorner.corner_3[2].y, 2) +
|
||||
pow(workpieceCorner.corner_3[0].z - workpieceCorner.corner_3[2].z, 2));
|
||||
sprintf_s(dataStr, 250, " Len: %g", dist);
|
||||
sw << dataStr << std::endl;
|
||||
}
|
||||
|
||||
if ((workpieceCorner.workpieceType == 1) || (workpieceCorner.workpieceType == 4))
|
||||
{
|
||||
sw << "D:" << std::endl;
|
||||
sprintf_s(dataStr, 250, " corner_0: (%g, %g, %g)", workpieceCorner.corner_4[0].x, workpieceCorner.corner_4[0].y, workpieceCorner.corner_4[0].z);
|
||||
sw << dataStr << std::endl;
|
||||
sprintf_s(dataStr, 250, " corner_1: (%g, %g, %g)", workpieceCorner.corner_4[1].x, workpieceCorner.corner_4[1].y, workpieceCorner.corner_4[1].z);
|
||||
sw << dataStr << std::endl;
|
||||
sprintf_s(dataStr, 250, " corner_2: (%g, %g, %g)", workpieceCorner.corner_4[2].x, workpieceCorner.corner_4[2].y, workpieceCorner.corner_4[2].z);
|
||||
sw << dataStr << std::endl;
|
||||
dist = sqrt(pow(workpieceCorner.corner_4[0].x - workpieceCorner.corner_4[2].x, 2) +
|
||||
pow(workpieceCorner.corner_4[0].y - workpieceCorner.corner_4[2].y, 2) +
|
||||
pow(workpieceCorner.corner_4[0].z - workpieceCorner.corner_4[2].z, 2));
|
||||
sprintf_s(dataStr, 250, " Len: %g", dist);
|
||||
sw << dataStr << std::endl;
|
||||
}
|
||||
sw << std::endl;
|
||||
|
||||
sprintf_s(dataStr, 250, "len_A1 : %g", workpieceCorner.len135_A1);
|
||||
sw << dataStr << std::endl;
|
||||
sprintf_s(dataStr, 250, "len_A2 : %g", workpieceCorner.len225_A2);
|
||||
sw << dataStr << std::endl;
|
||||
sprintf_s(dataStr, 250, "len_B2 : %g", workpieceCorner.len315_B2);
|
||||
sw << dataStr << std::endl;
|
||||
sprintf_s(dataStr, 250, "len_B1 : %g", workpieceCorner.len45_B1);
|
||||
sw << dataStr << std::endl;
|
||||
sw.close();
|
||||
}
|
||||
|
||||
@ -395,14 +413,14 @@ void _outputRGBDScanLapWeld_RGBD(
|
||||
std::vector<std::vector<SVzNL3DPosition>>& scanLines,
|
||||
SSX_BQworkpieceResult workpieceCorner,
|
||||
bool outDebugInfo,
|
||||
SSX_debugInfo* debugData)
|
||||
std::vector<SSX_debugInfo>& debugData)
|
||||
{
|
||||
int lineNum = (int)scanLines.size();
|
||||
std::ofstream sw(fileName);
|
||||
int realLines = lineNum;
|
||||
if (workpieceCorner.workpieceType > 0)
|
||||
realLines++;
|
||||
if(debugData)
|
||||
if(debugData.size() > 0)
|
||||
realLines++;
|
||||
sw << "LineNum:" << realLines << std::endl;
|
||||
sw << "DataType: 0" << std::endl;
|
||||
@ -472,24 +490,29 @@ void _outputRGBDScanLapWeld_RGBD(
|
||||
|
||||
if (workpieceCorner.workpieceType > 0)
|
||||
{
|
||||
int linePtNum = 12;
|
||||
std::vector<SVzNL3DPoint> ptBuffer;
|
||||
for (int i = 0; i < 3; i++)
|
||||
ptBuffer.push_back(workpieceCorner.corner_1[i]);
|
||||
for (int i = 0; i < 3; i++)
|
||||
ptBuffer.push_back(workpieceCorner.corner_2[i]);
|
||||
if (workpieceCorner.workpieceType != 3)
|
||||
{
|
||||
for (int i = 0; i < 3; i++)
|
||||
ptBuffer.push_back(workpieceCorner.corner_3[i]);
|
||||
}
|
||||
if ((workpieceCorner.workpieceType == 1) || (workpieceCorner.workpieceType == 4))
|
||||
{
|
||||
for (int i = 0; i < 3; i++)
|
||||
ptBuffer.push_back(workpieceCorner.corner_4[i]);
|
||||
}
|
||||
ptBuffer.push_back(workpieceCorner.center);
|
||||
int linePtNum = (int)ptBuffer.size();
|
||||
sw << "Line_" << lineNum << "_0_" << linePtNum + 1 << std::endl;
|
||||
lineNum++;
|
||||
|
||||
SVzNL3DPoint ptBuffer[12];
|
||||
int idx = 0;
|
||||
for (int i = 0; i < 3; i++)
|
||||
ptBuffer[idx++] = workpieceCorner.corner_L[i];
|
||||
for (int i = 0; i < 3; i++)
|
||||
ptBuffer[idx++] = workpieceCorner.corner_R[i];
|
||||
for (int i = 0; i < 3; i++)
|
||||
ptBuffer[idx++] = workpieceCorner.corner_T[i];
|
||||
for (int i = 0; i < 3; i++)
|
||||
ptBuffer[idx++] = workpieceCorner.corner_B[i];
|
||||
|
||||
rgb = { 255, 0, 0 };
|
||||
size = 15;
|
||||
for (int j = 0; j < 12; j++)
|
||||
for (int j = 0; j < linePtNum; j++)
|
||||
{
|
||||
float x = (float)ptBuffer[j].x;
|
||||
float y = (float)ptBuffer[j].y;
|
||||
@ -507,18 +530,52 @@ void _outputRGBDScanLapWeld_RGBD(
|
||||
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
|
||||
}
|
||||
|
||||
if ((debugData) && (workpieceCorner.workpieceType > 0))
|
||||
if ((debugData.size() > 0) && (workpieceCorner.workpieceType > 0))
|
||||
{
|
||||
int linePtNum = debugData[0].edge_size + debugData[0].edgeLink1_size + debugData[0].edgeLink2_size;
|
||||
linePtNum += debugData[1].edge_size + debugData[1].edgeLink1_size + debugData[1].edgeLink2_size;
|
||||
linePtNum += debugData[2].edge_size + debugData[2].edgeLink1_size + debugData[2].edgeLink2_size;
|
||||
linePtNum += debugData[3].edge_size + debugData[3].edgeLink1_size + debugData[3].edgeLink2_size;
|
||||
std::vector< SWD_3DPointPair> lines;
|
||||
SWD_3DPointPair a_line;
|
||||
a_line.pt1 = workpieceCorner.corner_1[0];
|
||||
a_line.pt2 = workpieceCorner.corner_1[2];
|
||||
lines.push_back(a_line);
|
||||
a_line.pt1 = workpieceCorner.corner_1[1];
|
||||
a_line.pt2 = workpieceCorner.center;
|
||||
lines.push_back(a_line);
|
||||
|
||||
a_line.pt1 = workpieceCorner.corner_2[0];
|
||||
a_line.pt2 = workpieceCorner.corner_2[2];
|
||||
lines.push_back(a_line);
|
||||
a_line.pt1 = workpieceCorner.corner_2[1];
|
||||
a_line.pt2 = workpieceCorner.center;
|
||||
lines.push_back(a_line);
|
||||
|
||||
if (workpieceCorner.workpieceType != 3)
|
||||
{
|
||||
a_line.pt1 = workpieceCorner.corner_3[0];
|
||||
a_line.pt2 = workpieceCorner.corner_3[2];
|
||||
lines.push_back(a_line);
|
||||
a_line.pt1 = workpieceCorner.corner_3[1];
|
||||
a_line.pt2 = workpieceCorner.center;
|
||||
lines.push_back(a_line);
|
||||
}
|
||||
if ((workpieceCorner.workpieceType == 1) || (workpieceCorner.workpieceType == 4))
|
||||
{
|
||||
a_line.pt1 = workpieceCorner.corner_4[0];
|
||||
a_line.pt2 = workpieceCorner.corner_4[2];
|
||||
lines.push_back(a_line);
|
||||
a_line.pt1 = workpieceCorner.corner_4[1];
|
||||
a_line.pt2 = workpieceCorner.center;
|
||||
lines.push_back(a_line);
|
||||
}
|
||||
|
||||
int linePtNum = 0;
|
||||
for (int i = 0; i < (int)debugData.size(); i++)
|
||||
linePtNum += debugData[i].edge_size + debugData[i].edgeLink1_size + debugData[i].edgeLink2_size;
|
||||
sw << "Line_" << lineNum << "_0_" << linePtNum + 1 << std::endl;
|
||||
lineNum++;
|
||||
|
||||
rgb = { 255, 0, 0 };
|
||||
size = 3;
|
||||
for (int i = 0; i < 4; i++)
|
||||
for (int i = 0; i < (int)debugData.size(); i++)
|
||||
{
|
||||
for (int j = 0; j < debugData[i].edge_size; j++)
|
||||
{
|
||||
@ -560,32 +617,10 @@ void _outputRGBDScanLapWeld_RGBD(
|
||||
rgb = { 255, 0, 0 };
|
||||
size = 3;
|
||||
int lineIdx = 0;
|
||||
for (int i = 0; i < 4; i++)
|
||||
for (int i = 0; i < (int)lines.size(); i++)
|
||||
{
|
||||
SVzNL3DPoint pt0 = debugData[i].edge_ends[0];
|
||||
SVzNL3DPoint pt1 = debugData[i].edge_ends[1];
|
||||
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++;
|
||||
|
||||
pt0 = debugData[i].edge_link1_ends[0];
|
||||
pt1 = debugData[i].edge_link1_ends[1];
|
||||
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++;
|
||||
|
||||
pt0 = debugData[i].edge_link2_ends[0];
|
||||
pt1 = debugData[i].edge_link2_ends[1];
|
||||
SVzNL3DPoint pt0 = lines[i].pt1;
|
||||
SVzNL3DPoint pt1 = lines[i].pt2;
|
||||
sw << "Poly_" << lineIdx << "_2" << std::endl;
|
||||
sw << "{" << (float)pt0.x << "," << (float)pt0.y << "," << (float)pt0.z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
@ -596,8 +631,8 @@ void _outputRGBDScanLapWeld_RGBD(
|
||||
lineIdx++;
|
||||
}
|
||||
//加一个直线，用于跳过显示工具bug
|
||||
SVzNL3DPoint pt0 = debugData[0].edge_ends[0];
|
||||
SVzNL3DPoint pt1 = debugData[0].edge_ends[1];
|
||||
SVzNL3DPoint pt0 = lines[0].pt1;
|
||||
SVzNL3DPoint pt1 = lines[0].pt2;
|
||||
sw << "Poly_" << lineIdx << "_2" << std::endl;
|
||||
sw << "{" << (float)pt0.x << "," << (float)pt0.y << "," << (float)pt0.z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
@ -783,7 +818,7 @@ int main()
|
||||
SSX_BQworkpiecePara workpieceParam;
|
||||
workpieceParam.lineLen = 180.0; //直线段长度
|
||||
int errCode = 0;
|
||||
SSX_debugInfo debug_conturs[4];
|
||||
std::vector<SSX_debugInfo> debug_conturs;
|
||||
SSX_BQworkpieceResult workpieceCorner = sx_BQ_getWorkpieceCorners(
|
||||
scanLines,
|
||||
cornerParam,
|
||||
@ -800,7 +835,7 @@ int main()
|
||||
//输出测试结果
|
||||
sprintf_s(_scan_file, "%sresult\\LaserLine%d_result.txt", dataPath[grp], fidx);
|
||||
#if _OUTPUT_DEBUG_DATA
|
||||
_outputRGBDScanLapWeld_RGBD(_scan_file, scanLines, workpieceCorner, true, debug_conturs);
|
||||
_outputRGBDScanLapWeld_RGBD(_scan_file, scanLines, workpieceCorner, false, debug_conturs);
|
||||
#else
|
||||
_outputRGBDScanLapWeld_RGBD(_scan_file, scanLines, workpieceCorner, true, NULL);
|
||||
#endif
|
||||
|
||||
@ -145,7 +145,7 @@
|
||||
<FunctionLevelLinking>true</FunctionLevelLinking>
|
||||
<IntrinsicFunctions>true</IntrinsicFunctions>
|
||||
<SDLCheck>true</SDLCheck>
|
||||
<PreprocessorDefinitions>NDEBUG;BQWORKPIECECORNEREXTRACTION_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||
<PreprocessorDefinitions>NDEBUG;BQWORKPIECECORNEREXTRACTION_EXPORTS;_WINDOWS;_USRDLL;SG_API_LIBRARY;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||
<ConformanceMode>true</ConformanceMode>
|
||||
<PrecompiledHeader>NotUsing</PrecompiledHeader>
|
||||
<PrecompiledHeaderFile>pch.h</PrecompiledHeaderFile>
|
||||
|
||||
@ -160,10 +160,10 @@
|
||||
<EnableUAC>false</EnableUAC>
|
||||
<AdditionalLibraryDirectories>..\..\thirdParty\opencv320\build\x64\vc14\lib;%(AdditionalLibraryDirectories)</AdditionalLibraryDirectories>
|
||||
<AdditionalDependencies>opencv_world320.lib;%(AdditionalDependencies)</AdditionalDependencies>
|
||||
<AdditionalOptions>/bigobj %(AdditionalOptions)</AdditionalOptions>
|
||||
</Link>
|
||||
</ItemDefinitionGroup>
|
||||
<ItemGroup>
|
||||
<ClInclude Include="..\sourceCode\SG_algo_Export.h" />
|
||||
<ClInclude Include="..\sourceCode\SG_baseAlgo_Export.h" />
|
||||
<ClInclude Include="..\sourceCode\SG_baseDataType.h" />
|
||||
<ClInclude Include="..\sourceCode\SG_errCode.h" />
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@ -13,10 +13,11 @@ typedef struct
|
||||
typedef struct
|
||||
{
|
||||
int workpieceType; //1-节点1； 2-节点2； 3-节点3；4-节点4；0-未知； 其它-非法
|
||||
SVzNL3DPoint corner_L[3];
|
||||
SVzNL3DPoint corner_R[3];
|
||||
SVzNL3DPoint corner_T[3];
|
||||
SVzNL3DPoint corner_B[3];
|
||||
//1,2,3,4为逆时针
|
||||
SVzNL3DPoint corner_1[3];
|
||||
SVzNL3DPoint corner_2[3];
|
||||
SVzNL3DPoint corner_3[3];
|
||||
SVzNL3DPoint corner_4[3];
|
||||
SVzNL3DPoint center; //工件中心点
|
||||
double len135_A1; //A1长度，从工件中心135度方向
|
||||
double len45_B1; //B1长度，从工件中心45度方向
|
||||
@ -27,15 +28,12 @@ typedef struct
|
||||
typedef struct
|
||||
{
|
||||
int rgnIdx;
|
||||
SVzNL3DPoint* edge;
|
||||
int edge_size;
|
||||
SVzNL3DPoint edge_ends[2];
|
||||
SVzNL3DPoint* edgeLink_1;
|
||||
int edgeLink1_size;
|
||||
SVzNL3DPoint edge_link1_ends[2];
|
||||
SVzNL3DPoint* edgeLink_2;
|
||||
int edgeLink2_size;
|
||||
SVzNL3DPoint edge_link2_ends[2];
|
||||
SVzNL3DPoint* edge;
|
||||
SVzNL3DPoint* edgeLink_1;
|
||||
SVzNL3DPoint* edgeLink_2;
|
||||
}SSX_debugInfo;
|
||||
|
||||
//读版本号
|
||||
@ -62,6 +60,6 @@ SG_APISHARED_EXPORT SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
|
||||
SSG_planeCalibPara groundCalibPara,
|
||||
SSX_BQworkpiecePara workpieceParam,
|
||||
#if _OUTPUT_DEBUG_DATA
|
||||
SSX_debugInfo* debug_conturs,
|
||||
std::vector<SSX_debugInfo>& debug_contours,
|
||||
#endif
|
||||
int* errCode);
|
||||
|
||||
@ -1,8 +1,10 @@
|
||||
#pragma once
|
||||
|
||||
#include "SG_algo_Export.h"
|
||||
#include <vector>
|
||||
#include <opencv2/opencv.hpp>
|
||||
|
||||
|
||||
//滤除离群点:z跳变门限方法（大于门限视为不连续，根据连续段点数量判断噪声）
|
||||
SG_APISHARED_EXPORT void sg_lineDataRemoveOutlier(
|
||||
SVzNL3DPosition* lineData,
|
||||
@ -343,10 +345,38 @@ SG_APISHARED_EXPORT void lineFitting_abc(
|
||||
double* _b,
|
||||
double* _c);
|
||||
|
||||
//计算Z均值
|
||||
SG_APISHARED_EXPORT double computeMeanZ(std::vector< SVzNL3DPoint>& pts);
|
||||
|
||||
//计算角度差值，在0-180度范围
|
||||
SG_APISHARED_EXPORT double computeAngleDiff(double theta1, double theta2);
|
||||
|
||||
//计算直线交点
|
||||
SG_APISHARED_EXPORT SVzNL3DPoint computeLineCrossPt_abs(
|
||||
double a1, double b1, double c1,
|
||||
double a2, double b2, double c2);
|
||||
|
||||
//计算过2点的直线方程
|
||||
SG_APISHARED_EXPORT void compute2ptLine(
|
||||
SVzNL3DPoint pt1,
|
||||
SVzNL3DPoint pt2,
|
||||
double* _a, double* _b, double* _c);
|
||||
|
||||
//旋转45度后的直线方程
|
||||
SG_APISHARED_EXPORT void rotateLine45Deg(
|
||||
double _a, double _b, double _c,
|
||||
double x0, double y0,
|
||||
double* r_a, double* r_b, double* r_c);
|
||||
|
||||
//计算直线角度
|
||||
SG_APISHARED_EXPORT double getLineAngle(double _a, double _b, double _c);
|
||||
|
||||
//计算两点的2D距离
|
||||
SG_APISHARED_EXPORT double compute2DLen(SVzNL3DPoint pt1, SVzNL3DPoint pt2);
|
||||
|
||||
//计算点到直线距离
|
||||
SG_APISHARED_EXPORT double computePtDistToLine(double x0, double y0, double a, double b, double c);
|
||||
|
||||
//计算垂足点，直线方程：y = kx + b
|
||||
SG_APISHARED_EXPORT SVzNL2DPointD sx_getFootPoint(
|
||||
double x0,
|
||||
|
||||
@ -478,3 +478,9 @@ typedef struct
|
||||
int markID;
|
||||
SVzNL3DPoint mark3D;
|
||||
}SWD_charuco3DMark;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
SVzNL3DPoint pt1;
|
||||
SVzNL3DPoint pt2;
|
||||
}SWD_3DPointPair;
|
||||
@ -237,6 +237,138 @@ void lineFitting(std::vector< SVzNL3DPoint>& inliers, double* _k, double* _b)
|
||||
*_b = (-x_sum * xy_sum + xx_sum * y_sum) / (num * xx_sum - x_sum * x_sum);//根据公式求解b
|
||||
}
|
||||
|
||||
//拟合成通用直线方程，包括垂直
|
||||
void lineFitting_abc(std::vector< SVzNL3DPoint>& inliers, double* _a, double* _b, double* _c)
|
||||
{
|
||||
//判断是否为垂直
|
||||
int dataSize = (int)inliers.size();
|
||||
if (dataSize <2)
|
||||
return;
|
||||
|
||||
double deltaX = abs(inliers[0].x - inliers[dataSize - 1].x);
|
||||
double deltaY = abs(inliers[0].y - inliers[dataSize - 1].y);
|
||||
std::vector< SVzNL3DPoint> fittingData;
|
||||
if (deltaX < deltaY)
|
||||
{
|
||||
//x=ky+b 拟合
|
||||
for (int i = 0; i < dataSize; i++)
|
||||
{
|
||||
SVzNL3DPoint a_fitPt;
|
||||
a_fitPt.x = inliers[i].y;
|
||||
a_fitPt.y = inliers[i].x;
|
||||
a_fitPt.z = inliers[i].z;
|
||||
fittingData.push_back(a_fitPt);
|
||||
}
|
||||
double k = 0, b = 0;
|
||||
lineFitting(fittingData, &k, &b);
|
||||
//ax+by+c
|
||||
*_a = 1.0;
|
||||
*_b = -k;
|
||||
*_c = -b;
|
||||
}
|
||||
else
|
||||
{
|
||||
//y = kx+b拟合
|
||||
double k = 0, b = 0;
|
||||
lineFitting(inliers, &k, &b);
|
||||
//ax+by+c
|
||||
*_a = k;
|
||||
*_b = -1;
|
||||
*_c = b;
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
//计算Z均值
|
||||
double computeMeanZ(std::vector< SVzNL3DPoint>& pts)
|
||||
{
|
||||
int ptNum = (int)pts.size();
|
||||
int vldNum = 0;
|
||||
double sumZ = 0;
|
||||
for (int i = 0; i < ptNum; i++)
|
||||
{
|
||||
if (pts[i].z > 1e-4)
|
||||
{
|
||||
sumZ += pts[i].z;
|
||||
vldNum++;
|
||||
}
|
||||
}
|
||||
if (vldNum > 0)
|
||||
return (sumZ / vldNum);
|
||||
else
|
||||
return 0;
|
||||
}
|
||||
|
||||
SVzNL3DPoint computeLineCrossPt_abs(double a1, double b1, double c1, double a2, double b2, double c2)
|
||||
{
|
||||
SVzNL3DPoint crossPt;
|
||||
crossPt.x = (c2 * b1 - c1 * b2) / (a1 * b2 - a2 * b1);
|
||||
crossPt.y = (c2 * a1 - c1 * a2) / (b1 * a2 - b2 * a1);
|
||||
crossPt.z = 0;
|
||||
return crossPt;
|
||||
}
|
||||
|
||||
//计算角度差值，在0-180度范围
|
||||
double computeAngleDiff(double theta1, double theta2)
|
||||
{
|
||||
double diff = theta1 - theta2;
|
||||
if (diff < 0)
|
||||
diff += 360;
|
||||
if (diff > 180)
|
||||
diff = 360 - diff;
|
||||
|
||||
return diff;
|
||||
}
|
||||
|
||||
void compute2ptLine(SVzNL3DPoint pt1, SVzNL3DPoint pt2, double* _a, double* _b, double* _c)
|
||||
{
|
||||
*_a = pt2.y - pt1.y;
|
||||
*_b = pt1.x - pt2.x;
|
||||
*_c = pt2.x * pt1.y - pt1.x * pt2.y;
|
||||
return;
|
||||
}
|
||||
|
||||
//旋转45度后的直线方程
|
||||
void rotateLine45Deg(
|
||||
double _a, double _b, double _c,
|
||||
double x0, double y0,
|
||||
double* r_a, double* r_b, double* r_c)
|
||||
{
|
||||
// 旋转后直线的系数（基于数学推导）
|
||||
*r_a = _a + _b;
|
||||
*r_b = _b - _a;
|
||||
*r_c = -(*r_a) * x0 - (*r_b) * y0;
|
||||
return;
|
||||
}
|
||||
|
||||
double getLineAngle(double _a, double _b, double _c)
|
||||
{
|
||||
if (_a = 0)
|
||||
return 0;
|
||||
else if (_b = 0)
|
||||
return 90;
|
||||
else
|
||||
{
|
||||
double theta = atan(-_a / _b) + PI / 2;
|
||||
theta = (theta * 180.0) / PI;
|
||||
return theta;
|
||||
}
|
||||
}
|
||||
|
||||
//计算两点的2D距离
|
||||
double compute2DLen(SVzNL3DPoint pt1, SVzNL3DPoint pt2)
|
||||
{
|
||||
double len = sqrt(pow(pt1.x - pt2.x, 2) + pow(pt1.y - pt2.y, 2));
|
||||
return len;
|
||||
}
|
||||
|
||||
double computePtDistToLine(double x0, double y0, double a, double b, double c)
|
||||
{
|
||||
double tmp = sqrt(pow(a, 2) + pow(b, 2));
|
||||
double dist = abs(a * x0 + b * y0 + c) / tmp;
|
||||
return dist;
|
||||
}
|
||||
|
||||
//计算垂足点，直线方程：y = kx + b
|
||||
SVzNL2DPointD sx_getFootPoint(double x0, double y0, double k, double b)
|
||||
{
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user