jerryzeng/algoLib
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Merge branch 'main' of http://gitea.mnutil.com/jerryzeng/algoLib

Browse Source
This commit is contained in:
杰仔 2026-04-14 15:41:44 +08:00
commit e05160dddb
7 changed files with 1016 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
bagPositioning/bagPositioning.vcxproj
View File

@ -42,13 +42,13 @@
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'" Label="Configuration"> <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'" Label="Configuration">
<ConfigurationType>DynamicLibrary</ConfigurationType> <ConfigurationType>DynamicLibrary</ConfigurationType>
<UseDebugLibraries>true</UseDebugLibraries> <UseDebugLibraries>true</UseDebugLibraries>
<PlatformToolset>v142</PlatformToolset> <PlatformToolset>v143</PlatformToolset>
<CharacterSet>Unicode</CharacterSet> <CharacterSet>Unicode</CharacterSet>
</PropertyGroup> </PropertyGroup>
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'" Label="Configuration"> <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'" Label="Configuration">
<ConfigurationType>DynamicLibrary</ConfigurationType> <ConfigurationType>DynamicLibrary</ConfigurationType>
<UseDebugLibraries>false</UseDebugLibraries> <UseDebugLibraries>false</UseDebugLibraries>
<PlatformToolset>v142</PlatformToolset> <PlatformToolset>v143</PlatformToolset>
<WholeProgramOptimization>true</WholeProgramOptimization> <WholeProgramOptimization>true</WholeProgramOptimization>
<CharacterSet>Unicode</CharacterSet> <CharacterSet>Unicode</CharacterSet>
</PropertyGroup> </PropertyGroup>

442
rodAndBarDetection_test/rodAndBarDetection_test.cpp
View File

@ -208,7 +208,7 @@ SSG_planeCalibPara _readCalibPara(char* fileName)
} }
void _outputChanneltInfo(char* fileName, std::vector<SSX_hexHeadScrewInfo>& screwInfo) void _outputChanneltInfo(char* fileName, std::vector<SSX_rodPoseInfo>& screwInfo)
{ {
std::ofstream sw(fileName); std::ofstream sw(fileName);
@ -216,14 +216,25 @@ void _outputChanneltInfo(char* fileName, std::vector<SSX_hexHeadScrewInfo>& scre
int objNum = (int)screwInfo.size(); int objNum = (int)screwInfo.size();
for (int i = 0; i < objNum; i++) for (int i = 0; i < objNum; i++)
{ {
sprintf_s(dataStr, 250, "螺杆_%d: center_( %g, %g, %g ), dir_( %g, %g, %g ), anlge_%g", sprintf_s(dataStr, 250, "螺杆_%d: center_( %g, %g, %g ), dir_( %g, %g, %g )",
i + 1, screwInfo[i].center.x, screwInfo[i].center.y, screwInfo[i].center.z, i + 1, screwInfo[i].center.x, screwInfo[i].center.y, screwInfo[i].center.z,
screwInfo[i].axialDir.x, screwInfo[i].axialDir.y, screwInfo[i].axialDir.z, screwInfo[i].rotateAngle); screwInfo[i].axialDir.x, screwInfo[i].axialDir.y, screwInfo[i].axialDir.z);
sw << dataStr << std::endl; sw << dataStr << std::endl;
} }
sw.close(); sw.close();
} }
void _outputPlatePiseInfo(char* fileName, SSX_pointPoseInfo& centerInfo)
{
std::ofstream sw(fileName);
char dataStr[250];
sprintf_s(dataStr, 250, "定位盘: center_( %g, %g, %g ), dir_( %g, %g, %g )",
centerInfo.center.x, centerInfo.center.y, centerInfo.center.z,
centerInfo.normalDir.x, centerInfo.normalDir.y, centerInfo.normalDir.z);
sw << dataStr << std::endl;
sw.close();
}
void _outputRodtInfo(char* fileName, std::vector<SSX_rodPositionInfo>& rodInfo) void _outputRodtInfo(char* fileName, std::vector<SSX_rodPositionInfo>& rodInfo)
{ {
std::ofstream sw(fileName); std::ofstream sw(fileName);
@ -241,10 +252,26 @@ void _outputRodtInfo(char* fileName, std::vector<SSX_rodPositionInfo>& rodInfo)
sw.close(); sw.close();
} }
void _outputWeldSeamInfo(char* fileName, std::vector<SSX_weldSeamInfo>& weldSeamInfo)
{
std::ofstream sw(fileName);
char dataStr[250];
int objNum = (int)weldSeamInfo.size();
for (int i = 0; i < objNum; i++)
{
sprintf_s(dataStr, 250, "螺杆_%d: center_( %g, %g, %g ), dir_normal_( %g, %g, %g )",
i + 1, weldSeamInfo[i].center.x, weldSeamInfo[i].center.y, weldSeamInfo[i].center.z,
weldSeamInfo[i].normalDir.x, weldSeamInfo[i].normalDir.y, weldSeamInfo[i].normalDir.z);
sw << dataStr << std::endl;
}
sw.close();
}
void _outputRGBDScan_RGBD( void _outputRGBDScan_RGBD(
char* fileName, char* fileName,
std::vector<std::vector<SVzNL3DPosition>>& scanLines, std::vector<std::vector<SVzNL3DPosition>>& scanLines,
std::vector<SSX_hexHeadScrewInfo>& screwInfo std::vector<SSX_rodPoseInfo>& screwInfo
) )
{ {
int lineNum = (int)scanLines.size(); int lineNum = (int)scanLines.size();
@ -377,6 +404,112 @@ void _outputRGBDScan_RGBD(
sw.close(); sw.close();
} }
void _outputRGBDScan_RGBD_centerPose(
char* fileName,
std::vector<std::vector<SVzNL3DPosition>>& scanLines,
SSX_pointPoseInfo& poseInfo
)
{
int lineNum = (int)scanLines.size();
std::ofstream sw(fileName);
int realLines = lineNum;
realLines += 1;
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 == 2)
{
rgb = { 250, 0, 0 };
size = 5;
}
//else if (pt3D->nPointIdx == 1)
//{
// rgb = { 0, 0, 250 };
// size = 3;
//}
else //if (pt3D->nPointIdx == 0)
{
rgb = { 200, 200, 200 };
size = 1;
}
float x = (float)pt3D->pt3D.x;
float y = (float)pt3D->pt3D.y;
float z = (float)pt3D->pt3D.z;
sw << "{" << x << "," << y << "," << z << "}-";
sw << "{0,0}-{0,0}-";
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
}
}
{
sw << "Line_" << lineIdx << "_0_1" << std::endl;
rgb = { 250, 0, 0 };
size = 8;
float x = (float)poseInfo.center.x;
float y = (float)poseInfo.center.y;
float z = (float)poseInfo.center.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;
lineIdx = 0;
{
SVzNL3DPoint pt0 = { poseInfo.center.x - len * poseInfo.normalDir.x,
poseInfo.center.y - len * poseInfo.normalDir.y,
poseInfo.center.z - len * poseInfo.normalDir.z };
SVzNL3DPoint pt1 = { poseInfo.center.x + len * poseInfo.normalDir.x,
poseInfo.center.y + len * poseInfo.normalDir.y,
poseInfo.center.z + len * poseInfo.normalDir.z };
//显示法向量
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();
}
void _outputRGBDScan_RGBD_rodInfo( void _outputRGBDScan_RGBD_rodInfo(
char* fileName, char* fileName,
std::vector<std::vector<SVzNL3DPosition>>& scanLines, std::vector<std::vector<SVzNL3DPosition>>& scanLines,
@ -511,6 +644,139 @@ void _outputRGBDScan_RGBD_rodInfo(
} }
sw.close(); sw.close();
} }
void _outputRGBDScan_RGBD_weldSeam(
char* fileName,
std::vector<std::vector<SVzNL3DPosition>>& scanLines,
std::vector<SSX_weldSeamInfo>& weldSeamInfo
)
{
int lineNum = (int)scanLines.size();
std::ofstream sw(fileName);
int realLines = lineNum;
int objNum = (int)weldSeamInfo.size();
if (objNum > 0)
realLines += 1;
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 centerFlag = pt3D->nPointIdx >> 4;
if (centerFlag > 0)
{
rgb = { 180, 0, 0 };
size = 2;
}
else
{
rgb = objColor[pt3D->nPointIdx % 8];
size = 2;
}
}
else //if (pt3D->nPointIdx == 0)
{
rgb = { 200, 200, 200 };
size = 1;
}
float x = (float)pt3D->pt3D.x;
float y = (float)pt3D->pt3D.y;
float z = (float)pt3D->pt3D.z;
sw << "{" << x << "," << y << "," << z << "}-";
sw << "{0,0}-{0,0}-";
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
}
}
if (objNum > 0)
{
sw << "Line_" << lineIdx << "_0_" << objNum << std::endl;
size = 12;
for (int i = 0; i < objNum; i++)
{
if (i == 0)
rgb = { 250, 255, 0 };
else
rgb = { 250, 0, 0 };
float x = (float)weldSeamInfo[i].center.x;
float y = (float)weldSeamInfo[i].center.y;
float z = (float)weldSeamInfo[i].center.z;
sw << "{" << x << "," << y << "," << z << "}-";
sw << "{0,0}-{0,0}-";
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
}
//输出法向
size = 8;
double len = 60;
lineIdx = 0;
for (int i = 0; i < objNum; i++)
{
if (i == 0)
rgb = { 250, 255, 0 };
else
rgb = { 250, 0, 0 };
SVzNL3DPoint pt0 = { weldSeamInfo[i].center.x, weldSeamInfo[i].center.y, weldSeamInfo[i].center.z };
SVzNL3DPoint pt2 = { weldSeamInfo[i].center.x + len * weldSeamInfo[i].normalDir.x,
weldSeamInfo[i].center.y + len * weldSeamInfo[i].normalDir.y,
weldSeamInfo[i].center.z + len * weldSeamInfo[i].normalDir.z };
//显示轴向量
sw << "Poly_" << lineIdx << "_2" << std::endl;
sw << "{" << (float)weldSeamInfo[i].startPt.x << "," << (float)weldSeamInfo[i].startPt.y << "," << (float)weldSeamInfo[i].startPt.z << "}-";
sw << "{0,0}-{0,0}-";
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
sw << "{" << (float)weldSeamInfo[i].endPt.x << "," << (float)weldSeamInfo[i].endPt.y << "," << (float)weldSeamInfo[i].endPt.z << "}-";
sw << "{0,0}-{0,0}-";
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
lineIdx++;
//显示法向量
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 << "{" << (float)pt2.x << "," << (float)pt2.y << "," << (float)pt2.z << "}-";
sw << "{0,0}-{0,0}-";
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
lineIdx++;
}
lineIdx++;
}
sw.close();
}
#define SCREW_TEST_GROUP 1 #define SCREW_TEST_GROUP 1
void screwTest(void) void screwTest(void)
{ {
@ -566,7 +832,7 @@ void screwTest(void)
bool isHorizonScan = true; //true:激光线平行槽道；false:激光线垂直槽道 bool isHorizonScan = true; //true:激光线平行槽道；false:激光线垂直槽道
int errCode = 0; int errCode = 0;
std::vector<SSX_hexHeadScrewInfo> screwInfo; std::vector<SSX_rodPoseInfo> screwInfo;
sx_hexHeadScrewMeasure( sx_hexHeadScrewMeasure(
scanLines, scanLines,
isHorizonScan, //true:激光线平行槽道；false:激光线垂直槽道 isHorizonScan, //true:激光线平行槽道；false:激光线垂直槽道
@ -588,6 +854,64 @@ void screwTest(void)
} }
} }
#define LOCATING_PALTE_TEST_GROUP 1
void locatingPlateTest(void)
{
const char* dataPath[SCREW_TEST_GROUP] = {
"F:/ShangGu/项目/冠钦项目/螺杆测量/配天现场点云/定位盘点云/", //0
};
SVzNLRange fileIdx[LOCATING_PALTE_TEST_GROUP] = {
{1,16},
};
const char* ver = wd_rodAndBarDetectionVersion();
printf("ver:%s\n", ver);
for (int grp = 0; grp < LOCATING_PALTE_TEST_GROUP; grp++)
{
for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
{
//fidx =4;
char _scan_file[256];
sprintf_s(_scan_file, "%sLaserData_%d.txt", dataPath[grp], fidx);
std::vector<std::vector< SVzNL3DPosition>> scanLines;
wdReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines);
//转成plyTxt格式
//sprintf_s(_scan_file, "%s%d_ply_Hi229229.txt", dataPath[grp], fidx);
//wdSavePlyTxt(_scan_file, scanLines);
long t1 = (long)GetTickCount64();//统计时间
double rodDiameter = 10.0;
SSG_cornerParam cornerParam;
cornerParam.cornerTh = 60; //45度角
cornerParam.scale = rodDiameter / 4; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8;
cornerParam.minEndingGap = 20; // algoParam.bagParam.bagW / 4;
cornerParam.minEndingGap_z = 5.0;
cornerParam.jumpCornerTh_1 = 15; //水平角度，小于此角度视为水平
cornerParam.jumpCornerTh_2 = 60;
int errCode = 0;
SSX_pointPoseInfo centerPose = sx_getLocationPlatePose(
scanLines,
cornerParam,
& 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_RGBD_centerPose(_scan_file, scanLines, centerPose);
sprintf_s(_scan_file, "%sresult\\%d_screw_info.txt", dataPath[grp], fidx);
_outputPlatePiseInfo(_scan_file, centerPose);
}
}
}
#define ROD_POSITION_TEST_GROUP 1 #define ROD_POSITION_TEST_GROUP 1
void rodPositionTest(void) void rodPositionTest(void)
{ {
@ -685,12 +1009,118 @@ void rodPositionTest(void)
} }
} }
#define WELD_SEAM_TEST_GROUP 2
void rodWeldSeamPosition_test(void)
{
const char* dataPath[WELD_SEAM_TEST_GROUP] = {
"F:/ShangGu/项目/冠钦项目/筑裕视觉钢结构焊接/视觉照片1/", //0
"F:/ShangGu/项目/冠钦项目/筑裕视觉钢结构焊接/视觉照片2/", //0
};
SVzNLRange fileIdx[WELD_SEAM_TEST_GROUP] = {
{1,20},{1,20},
};
const char* ver = wd_rodAndBarDetectionVersion();
printf("ver:%s\n", ver);
for (int grp = 0; grp < WELD_SEAM_TEST_GROUP; grp++)
{
SSG_planeCalibPara poseCalibPara;
//初始化成单位阵
poseCalibPara.planeCalib[0] = 1.0;
poseCalibPara.planeCalib[1] = 0.0;
poseCalibPara.planeCalib[2] = 0.0;
poseCalibPara.planeCalib[3] = 0.0;
poseCalibPara.planeCalib[4] = 1.0;
poseCalibPara.planeCalib[5] = 0.0;
poseCalibPara.planeCalib[6] = 0.0;
poseCalibPara.planeCalib[7] = 0.0;
poseCalibPara.planeCalib[8] = 1.0;
poseCalibPara.planeHeight = -1.0;
for (int i = 0; i < 9; i++)
poseCalibPara.invRMatrix[i] = poseCalibPara.planeCalib[i];
//char calibFile[250];
//sprintf_s(calibFile, "%sground_calib_para.txt", dataPath[grp]);
//poseCalibPara = _readCalibPara(calibFile);
for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
{
//fidx =1;
char _scan_file[256];
sprintf_s(_scan_file, "%sLaserData_%d.txt", dataPath[grp], fidx);
std::vector<std::vector< SVzNL3DPosition>> scanLines;
wdReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines);
//转成plyTxt格式
//sprintf_s(_scan_file, "%s%d_ply_Hi229229.txt", dataPath[grp], fidx);
//wdSavePlyTxt(_scan_file, scanLines);
long t1 = (long)GetTickCount64();//统计时间
SSX_rodParam rodParam;
rodParam.diameter = 52.0; //圆棒直径
rodParam.len = 290;
SSG_cornerParam cornerParam;
cornerParam.cornerTh = 60; //45度角
cornerParam.scale = rodParam.diameter / 4; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8;
cornerParam.minEndingGap = 20; // algoParam.bagParam.bagW / 4;
cornerParam.minEndingGap_z = 5.0;
cornerParam.jumpCornerTh_1 = 15; //水平角度，小于此角度视为水平
cornerParam.jumpCornerTh_2 = 60;
SSG_outlierFilterParam filterParam;
filterParam.continuityTh = 5.0; //噪声滤除。当相邻点的z跳变大于此门限时，检查是否为噪声。若长度小于outlierLen， 视为噪声
filterParam.outlierTh = 5;
SSG_treeGrowParam growParam;
growParam.maxLineSkipNum = 5;
growParam.yDeviation_max = 10.0;
growParam.maxSkipDistance = 10.0;
growParam.zDeviation_max = 10.0;//
growParam.minLTypeTreeLen = 100; //mm, 螺杆长度
growParam.minVTypeTreeLen = 100; //mm
bool isHorizonScan = true; //true:激光线平行槽道；false:激光线垂直槽道
int errCode = 0;
std::vector<SSX_weldSeamInfo> weldSeamInfo;
sx_rebarWeldSeamPositioning(
scanLines,
poseCalibPara,
cornerParam,
filterParam,
growParam,
rodParam,
weldSeamInfo,
&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_RGBD_weldSeam(_scan_file, scanLines, weldSeamInfo);
sprintf_s(_scan_file, "%sresult\\%d_screw_info.txt", dataPath[grp], fidx);
_outputWeldSeamInfo(_scan_file, weldSeamInfo);
}
}
}
int main() int main()
{ {
#if 0 //螺杆定位测试 #if 1 //螺杆定位测试
#if 0
screwTest(); screwTest();
#else
locatingPlateTest();
#endif
#else //棒材抓取定位测试 #else //棒材抓取定位测试
#if 0
rodPositionTest(); rodPositionTest();
#else
rodWeldSeamPosition_test();
#endif
#endif #endif
return 0; return 0;
} }

13
sourceCode/SG_baseAlgo_Export.h
View File

@ -139,6 +139,14 @@ SG_APISHARED_EXPORT void wd_getLineCornerFeature_PSM(
const SSG_cornerParam cornerPara, const SSG_cornerParam cornerPara,
SSG_lineFeature* line_features); SSG_lineFeature* line_features);
SG_APISHARED_EXPORT void wd_lineDataSegment_zDist(
std::vector< SVzNL3DPosition>& lineData,
std::vector< SVzNL3DPosition>& vldPts,
std::vector<SSG_RUN_EX>& segs,
std::vector<int>& backIndexing,
const SSG_cornerParam cornerPara
);
SG_APISHARED_EXPORT void wd_computeDirAngle_wholeLine( SG_APISHARED_EXPORT void wd_computeDirAngle_wholeLine(
std::vector< SVzNL3DPosition>& line_data, std::vector< SVzNL3DPosition>& line_data,
const SSG_cornerParam cornerPara, const SSG_cornerParam cornerPara,
@ -643,8 +651,8 @@ SG_APISHARED_EXPORT void vzCaculateLaserPlane(
SG_APISHARED_EXPORT Plane robustFitPlane( SG_APISHARED_EXPORT Plane robustFitPlane(
const std::vector<cv::Point3f>& points, const std::vector<cv::Point3f>& points,
RobustType type = TUKEY, RobustType type = HUBER,
double delta = 0.5, // 阈值：>此值视为离群点（mm） double delta = 1.0, // 阈值：>此值视为离群点（mm）
int maxIter = 20, // 迭代次数 int maxIter = 20, // 迭代次数
double convergeThresh = 1e-5 // 收敛阈值（平面变化足够小就停） double convergeThresh = 1e-5 // 收敛阈值（平面变化足够小就停）
); );
@ -740,6 +748,7 @@ SG_APISHARED_EXPORT void wd_pointClustering_speedUp(
std::vector< SVzNL3DPosition>& pts, std::vector< SVzNL3DPosition>& pts,
int lineNum, int ptSize, int clusterCheckWin, //搜索窗口 int lineNum, int ptSize, int clusterCheckWin, //搜索窗口
double clusterDist, double clusterDist,
int distType, //0 - 2d distance; 1- 3d distance
std::vector<std::vector< SVzNL3DPosition>>& objClusters //result std::vector<std::vector< SVzNL3DPosition>>& objClusters //result
); );

12
sourceCode/SG_clustering.cpp
View File

@ -41,6 +41,7 @@ void _seedClustering_speedUp(
std::vector< SVzNL3DPosition>& pts, std::vector< SVzNL3DPosition>& pts,
std::vector<int>& flags, std::vector<int>& flags,
double clusterDist, double clusterDist,
int distType, //0 - 2d distance; 1- 3d distance
std::vector<std::vector<int>>& indexing2D, std::vector<std::vector<int>>& indexing2D,
int lineNum, int ptSize, int clusterCheckWin) //ËÑË÷´°¿Ú int lineNum, int ptSize, int clusterCheckWin) //ËÑË÷´°¿Ú
{ {
@ -66,7 +67,11 @@ void _seedClustering_speedUp(
continue; continue;
if (flags[backIdx] < 0) if (flags[backIdx] < 0)
continue; continue;
double dist = sqrt(pow(a_seed.pt3D.x - pts[backIdx].pt3D.x,2) + pow(a_seed.pt3D.y - pts[backIdx].pt3D.y,2)); double dist;
if(0 == distType)
dist = sqrt(pow(a_seed.pt3D.x - pts[backIdx].pt3D.x, 2) + pow(a_seed.pt3D.y - pts[backIdx].pt3D.y, 2));
else
dist = sqrt(pow(a_seed.pt3D.x - pts[backIdx].pt3D.x, 2) + pow(a_seed.pt3D.y - pts[backIdx].pt3D.y, 2) + pow(a_seed.pt3D.z - pts[backIdx].pt3D.z, 2));
if (dist < clusterDist) if (dist < clusterDist)
{ {
a_cluster.push_back(pts[backIdx]); a_cluster.push_back(pts[backIdx]);
@ -129,6 +134,7 @@ void wd_pointClustering_speedUp(
std::vector< SVzNL3DPosition>& pts, std::vector< SVzNL3DPosition>& pts,
int lineNum, int linePtSize, int clusterCheckWin, //ËÑË÷´°¿Ú int lineNum, int linePtSize, int clusterCheckWin, //ËÑË÷´°¿Ú
double clusterDist, double clusterDist,
int distType, //0 - 2d distance; 1- 3d distance
std::vector<std::vector< SVzNL3DPosition>>& objClusters //result std::vector<std::vector< SVzNL3DPosition>>& objClusters //result
) )
{ {
@ -170,9 +176,11 @@ void wd_pointClustering_speedUp(
pts, pts,
flags, flags,
clusterDist, clusterDist,
distType,
indexing2D, indexing2D,
lineNum, linePtSize, clusterCheckWin); lineNum, linePtSize, clusterCheckWin);
objClusters.push_back(a_cluster); //保存一个聚类 if(a_cluster.size() >= 5)
objClusters.push_back(a_cluster); //±£´æÒ»¸ö¾ÛÀà
} }
idx++; idx++;
} }

4
sourceCode/SG_lineFeature.cpp
View File

@ -3825,7 +3825,7 @@ bool compareByPtIdx(const SSG_basicFeature1D& a, const SSG_basicFeature1D& b) {
} }
//根据Z连续性分段 //根据Z连续性分段
void _lineDataSegment_zDist( void wd_lineDataSegment_zDist(
std::vector< SVzNL3DPosition>& lineData, std::vector< SVzNL3DPosition>& lineData,
std::vector< SVzNL3DPosition>& vldPts, std::vector< SVzNL3DPosition>& vldPts,
std::vector<SSG_RUN_EX>& segs, std::vector<SSG_RUN_EX>& segs,
@ -4089,7 +4089,7 @@ void wd_getRingArcFeature(
backIndexing.resize(dataSize); backIndexing.resize(dataSize);
//根据z连续性分段 //根据z连续性分段
_lineDataSegment_zDist(lineData, vldPts, segs, backIndexing, cornerPara); wd_lineDataSegment_zDist(lineData, vldPts, segs, backIndexing, cornerPara);
//计算前向角和后向角 //计算前向角和后向角
std::vector< SSG_pntDirAngle> ptDirAngles; std::vector< SSG_pntDirAngle> ptDirAngles;
_computeDirAngle_perSeg(vldPts, segs, cornerPara, ptDirAngles); _computeDirAngle_perSeg(vldPts, segs, cornerPara, ptDirAngles);

525
sourceCode/rodAndBarDetection.cpp
View File

@ -8,7 +8,8 @@
//version 1.0.0 : base version release to customer //version 1.0.0 : base version release to customer
//version 1.1.0 : 添加了地面调平和棒材定位 //version 1.1.0 : 添加了地面调平和棒材定位
//version 1.1.1 : 初始发布给客户的版本 //version 1.1.1 : 初始发布给客户的版本
std::string m_strVersion = "1.1.1"; //version 1.2.0 : 配天螺杆测量增加了定位盘中心测量功能
std::string m_strVersion = "1.2.0";
const char* wd_rodAndBarDetectionVersion(void) const char* wd_rodAndBarDetectionVersion(void)
{ {
return m_strVersion.c_str(); return m_strVersion.c_str();
@ -204,6 +205,7 @@ SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, double matrix3d[9])
return _r_pt; return _r_pt;
} }
//计算螺杆端部中心点位姿
void sx_hexHeadScrewMeasure( void sx_hexHeadScrewMeasure(
std::vector< std::vector<SVzNL3DPosition>>& scanLines, std::vector< std::vector<SVzNL3DPosition>>& scanLines,
bool isHorizonScan, //true:激光线平行槽道；false:激光线垂直槽道 bool isHorizonScan, //true:激光线平行槽道；false:激光线垂直槽道
@ -211,7 +213,7 @@ SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, double matrix3d[9])
const SSG_outlierFilterParam filterParam, const SSG_outlierFilterParam filterParam,
const SSG_treeGrowParam growParam, const SSG_treeGrowParam growParam,
double rodDiameter, double rodDiameter,
std::vector<SSX_hexHeadScrewInfo>& screwInfo, std::vector<SSX_rodPoseInfo>& screwInfo,
int* errCode) int* errCode)
{ {
*errCode = 0; *errCode = 0;
@ -429,10 +431,9 @@ SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, double matrix3d[9])
//旋转回原坐标系 //旋转回原坐标系
SVzNL3DPoint surfaceCenter = _ptRotate(projectionCenter, rotatePara.invRMatrix); SVzNL3DPoint surfaceCenter = _ptRotate(projectionCenter, rotatePara.invRMatrix);
//生成Rod信息 //生成Rod信息
SSX_hexHeadScrewInfo a_rod; SSX_rodPoseInfo a_rod;
a_rod.center = surfaceCenter; a_rod.center = surfaceCenter;
a_rod.axialDir = P1_dir; a_rod.axialDir = P1_dir;
a_rod.rotateAngle = 0;
screwInfo.push_back(a_rod); screwInfo.push_back(a_rod);
} }
if (true == isHorizonScan) if (true == isHorizonScan)
@ -452,6 +453,402 @@ SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, double matrix3d[9])
return; return;
} }
double _getListMeanZ(std::vector< SVzNL3DPosition>& listData)
{
if (listData.size() == 0)
return 0;
double meanZ = 0;
for (int i = 0; i < (int)listData.size(); i++)
meanZ += listData[i].pt3D.z;
meanZ = meanZ / (double)listData.size();
return meanZ;
}
SSG_ROIRectD _getListROI(std::vector< SVzNL3DPosition>& listData)
{
if (listData.size() == 0)
return { 0,0,0,0 };
SSG_ROIRectD roi = { listData[0].pt3D.x, listData[0].pt3D.x, listData[0].pt3D.y, listData[0].pt3D.y};
for (int i = 0; i < (int)listData.size(); i++)
{
roi.left = roi.left > listData[i].pt3D.x ? listData[i].pt3D.x : roi.left;
roi.right = roi.right < listData[i].pt3D.x ? listData[i].pt3D.x : roi.right;
roi.top = roi.top > listData[i].pt3D.y ? listData[i].pt3D.y : roi.top;
roi.bottom = roi.bottom < listData[i].pt3D.y ? listData[i].pt3D.y : roi.bottom;
}
return roi;
}
//计算定位盘中心点位姿
SSX_pointPoseInfo sx_getLocationPlatePose(
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
const SSG_cornerParam cornerPara,
int* errCode)
{
*errCode = 0;
SSX_pointPoseInfo resultPose;
resultPose.center = { 0, 0, 0 };
resultPose.normalDir = { 0, 0, 0 };
int lineNum = (int)scanLines.size();
if (lineNum == 0)
{
*errCode = SG_ERR_3D_DATA_NULL;
return resultPose;
}
int linePtNum = (int)scanLines[0].size();
//判断数据格式是否为grid。算法只能处理grid数据格式
bool isGridData = true;
for (int line = 0; line < lineNum; line++)
{
if (linePtNum != (int)scanLines[line].size())
{
isGridData = false;
break;
}
}
if (false == isGridData)//数据不是网格格式
{
*errCode = SG_ERR_NOT_GRID_FORMAT;
return resultPose;
}
//产生水平扫描数据
std::vector< std::vector<SVzNL3DPosition>> scanLines_h;
scanLines_h.resize(linePtNum);
for (int i = 0; i < linePtNum; i++)
scanLines_h[i].resize(lineNum);
for (int line = 0; line < lineNum; line++)
{
for (int j = 0; j < linePtNum; j++)
{
scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
scanLines_h[j][line] = scanLines[line][j];
scanLines_h[j][line].pt3D.x = scanLines[line][j].pt3D.y;
scanLines_h[j][line].pt3D.y = scanLines[line][j].pt3D.x;
}
}
for (int line = 0; line < linePtNum; line++)
{
for (int j = 0, j_max = (int)scanLines_h[line].size(); j < j_max; j++)
scanLines_h[line][j].nPointIdx = j;
}
//获取定位盘外表面端点
//垂直方向
std::vector<std::vector<int>> flags;
flags.resize(lineNum);
for (int i = 0; i < lineNum; i++)
{
flags[i].resize(linePtNum);
std::fill(flags[i].begin(), flags[i].end(), 0);
}
std::vector< SVzNL3DPosition> endingPts;
for (int line = 0; line < lineNum; line++)
{
std::vector< SVzNL3DPosition> vldPts;
std::vector<SSG_RUN_EX> segs;
std::vector<int> backIndexing;
backIndexing.resize(scanLines[line].size());
wd_lineDataSegment_zDist(
scanLines[line],
vldPts,
segs,
backIndexing,
cornerPara
);
for (int i = 0; i < (int)segs.size(); i++)
{
int idx_0 = segs[i].start;
int idx_1 = segs[i].start + segs[i].len - 1;
SVzNL3DPosition pt_0 = vldPts[idx_0];
SVzNL3DPosition pt_1 = vldPts[idx_1];
flags[line][pt_0.nPointIdx] = 1;
flags[line][pt_1.nPointIdx] = 1;
pt_0.nPointIdx = pt_0.nPointIdx & 0xffff | (line << 16);
pt_1.nPointIdx = pt_1.nPointIdx & 0xffff | (line << 16);
endingPts.push_back(pt_0);
endingPts.push_back(pt_1);
}
}
for (int line = 0; line < linePtNum; line++)
{
std::vector< SVzNL3DPosition> vldPts;
std::vector<SSG_RUN_EX> segs;
std::vector<int> backIndexing;
backIndexing.resize(scanLines_h[line].size());
wd_lineDataSegment_zDist(
scanLines_h[line],
vldPts,
segs,
backIndexing,
cornerPara
);
for (int i = 0; i < (int)segs.size(); i++)
{
int idx_0 = segs[i].start;
int idx_1 = segs[i].start + segs[i].len - 1;
SVzNL3DPosition pt_0 = vldPts[idx_0];
SVzNL3DPosition pt_1 = vldPts[idx_1];
if (0 == flags[pt_0.nPointIdx][line])//不和垂直特征提取的点重复
{
flags[pt_0.nPointIdx][line] = 1;
pt_0.pt3D = scanLines[pt_0.nPointIdx][line].pt3D; //取原始点
pt_0.nPointIdx = (pt_0.nPointIdx <<16) | (line & 0xffff);
endingPts.push_back(pt_0);
}
if (0 == flags[pt_1.nPointIdx][line]) //不和垂直特征提取的点重复
{
flags[pt_1.nPointIdx][line] = 1;
pt_1.pt3D = scanLines[pt_1.nPointIdx][line].pt3D; //取原始点
pt_1.nPointIdx = (pt_1.nPointIdx << 16) | (line & 0xffff);
endingPts.push_back(pt_1);
}
}
}
//标注
for (int line = 0; line < lineNum; line++)
{
for (int j = 0; j < linePtNum; j++)
scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
}
for (int i = 0; i < (int)endingPts.size(); i++)
{
int line = endingPts[i].nPointIdx >> 16;
int ptIdx = endingPts[i].nPointIdx & 0x0000FFFF;
scanLines[line][ptIdx].nPointIdx = 1;
}
//聚类
//内部参数
int clusterCheckWin = 5;
double clusterDist = 5.0;
double topLayerThickness = 10.0;
double centerZ_R = 20.0;
int distType = 1; //0 - 2d distance; 1- 3d distance
std::vector<std::vector< SVzNL3DPosition>> objClusters; //result
wd_pointClustering_speedUp(
endingPts,
lineNum, linePtNum, clusterCheckWin, //搜索窗口
clusterDist,
distType,
objClusters //result
);
//判断上表面外轮廓
std::vector<double> objMeanZ;
objMeanZ.resize(objClusters.size());
std::vector< SSG_ROIRectD> objROIs;
objROIs.resize(objClusters.size());
//过滤出最小范围的Z
double minMeanZ = -1;
for (int i = 0; i < (int)objClusters.size(); i++)
{
SSG_ROIRectD a_roi = _getListROI(objClusters[i]);
objROIs[i] = a_roi;
double w = a_roi.right - a_roi.left;
double h = a_roi.bottom - a_roi.top;
double meanZ = _getListMeanZ(objClusters[i]);
objMeanZ[i] = meanZ;
if ( (meanZ > 1e-4) && (w > 150) && (h > 100))
{
if (minMeanZ < 0)
minMeanZ = meanZ;
else
minMeanZ = minMeanZ > meanZ ? meanZ : minMeanZ;
}
}
//选出ROI最大的目标作为顶部轮廓
int objIdx = -1;
double maxSize = 0;
for (int i = 0; i < (int)objClusters.size(); i++)
{
if (objMeanZ[i] > 1e-4)
{
double zDist = objMeanZ[i] - minMeanZ;
if (zDist < topLayerThickness) //同在顶层
{
double size = (objROIs[i].right - objROIs[i].left) * (objROIs[i].bottom - objROIs[i].top);
if (maxSize < -1e-4)
{
maxSize = size;
objIdx = i;
}
else
{
if (maxSize < size)
{
maxSize = size;
objIdx = i;
}
}
}
}
}
if (objIdx < 0)
{
*errCode = SG_ERR_ZERO_OBJECTS;
return resultPose;
}
//标注
for (int i = 0; i < (int)objClusters[objIdx].size(); i++)
{
int line = objClusters[objIdx][i].nPointIdx >> 16;
int ptIdx = objClusters[objIdx][i].nPointIdx & 0x0000FFFF;
scanLines[line][ptIdx].nPointIdx = 2;
}
//拟合平面
std::vector<cv::Point3f> Points3ds;
for (int i = 0; i < (int)objClusters[objIdx].size(); i++)
{
cv::Point3f a_pt = cv::Point3f(objClusters[objIdx][i].pt3D.x, objClusters[objIdx][i].pt3D.y, objClusters[objIdx][i].pt3D.z);
Points3ds.push_back(a_pt);
}
//计算面参数: z = Ax + By + C
//res: [0]=A, [1]= B, [2]=-1.0, [3]=C,
//std::vector<cv::Point3f> out_inliers;
//Plane res = ransacFitPlane( Points3ds, out_inliers );
Plane res = robustFitPlane(Points3ds);
//std::vector<double> res;
//vzCaculateLaserPlane(Points3ds, res);
//计算投影向量
SVzNL3DPoint vec_1 = {res.A, res.B, res.C};
SVzNL3DPoint vec_2 = { 0, 0, 1.0 };
SSG_planeCalibPara poseR = wd_computeRTMatrix(vec_1, vec_2);
//投影
std::vector<cv::Point3f> projectPoints3ds;
projectPoints3ds.resize(Points3ds.size());
double sum_x = 0, sum_y = 0;
for (int i = 0; i < (int)Points3ds.size(); i++)
{
double x = Points3ds[i].x * poseR.planeCalib[0] + Points3ds[i].y * poseR.planeCalib[1] + Points3ds[i].z * poseR.planeCalib[2];
double y = Points3ds[i].x * poseR.planeCalib[3] + Points3ds[i].y * poseR.planeCalib[4] + Points3ds[i].z * poseR.planeCalib[5];
double z = Points3ds[i].x * poseR.planeCalib[6] + Points3ds[i].y * poseR.planeCalib[7] + Points3ds[i].z * poseR.planeCalib[8];
projectPoints3ds[i] = cv::Point3f(x, y, z);
sum_x += x;
sum_y += y;
}
for (int i = 0; i < lineNum; i++)
{
if (i == 14)
int kkk = 1;
//行处理
//调平，去除地面
lineDataRT_vector(scanLines[i], poseR.planeCalib, -1);
}
//计算质心
double center_x = sum_x / (double)Points3ds.size();
double center_y = sum_y / (double)Points3ds.size();
//计算4个点
SVzNL2DPointD top_pt = { center_x, center_y - centerZ_R };
SVzNL2DPointD bottom_pt = { center_x, center_y + centerZ_R };
SVzNL2DPointD left_pt = { center_x - centerZ_R, center_y};
SVzNL2DPointD roght_pt = { center_x + centerZ_R, center_y};
//计算Z
SVzNL3DPoint ptTop = { 0, 0, 0 };
SVzNL3DPoint ptBtm = { 0, 0, 0 };
SVzNL3DPoint ptLeft = { 0, 0, 0 };
SVzNL3DPoint ptRight = { 0, 0, 0 };
double minDistTop = -1, minDistBtm = -1, minDistLeft = -1, minDistRight = -1;
for (int line = 0; line < lineNum; line++)
{
for (int j = 0; j < linePtNum; j++)
{
if (scanLines[line][j].pt3D.z > 1e-4)
{
//top
if (minDistTop < 1e-4)
{
ptTop = scanLines[line][j].pt3D;
minDistTop = sqrt(pow(top_pt.x - scanLines[line][j].pt3D.x, 2) + pow(top_pt.y - scanLines[line][j].pt3D.y, 2));
}
else
{
double dist = sqrt(pow(top_pt.x - scanLines[line][j].pt3D.x, 2) + pow(top_pt.y - scanLines[line][j].pt3D.y, 2));
if (minDistTop > dist)
{
ptTop = scanLines[line][j].pt3D;
minDistTop = dist;
}
}
//bottom
if (minDistBtm < 1e-4)
{
ptBtm = scanLines[line][j].pt3D;
minDistBtm = sqrt(pow(bottom_pt.x - scanLines[line][j].pt3D.x, 2) + pow(bottom_pt.y - scanLines[line][j].pt3D.y, 2));
}
else
{
double dist = sqrt(pow(bottom_pt.x - scanLines[line][j].pt3D.x, 2) + pow(bottom_pt.y - scanLines[line][j].pt3D.y, 2));
if (minDistBtm > dist)
{
ptBtm = scanLines[line][j].pt3D;
minDistBtm = dist;
}
}
//left
if (minDistLeft < 1e-4)
{
ptLeft = scanLines[line][j].pt3D;
minDistLeft = sqrt(pow(left_pt.x - scanLines[line][j].pt3D.x, 2) + pow(left_pt.y - scanLines[line][j].pt3D.y, 2));
}
else
{
double dist = sqrt(pow(left_pt.x - scanLines[line][j].pt3D.x, 2) + pow(left_pt.y - scanLines[line][j].pt3D.y, 2));
if (minDistLeft > dist)
{
ptLeft = scanLines[line][j].pt3D;
minDistLeft = dist;
}
}
//right
if (minDistRight < 1e-4)
{
ptRight = scanLines[line][j].pt3D;
minDistRight = sqrt(pow(roght_pt.x - scanLines[line][j].pt3D.x, 2) + pow(roght_pt.y - scanLines[line][j].pt3D.y, 2));
}
else
{
double dist = sqrt(pow(roght_pt.x - scanLines[line][j].pt3D.x, 2) + pow(roght_pt.y - scanLines[line][j].pt3D.y, 2));
if (minDistRight > dist)
{
ptRight = scanLines[line][j].pt3D;
minDistRight = dist;
}
}
}
}
}
double center_z = (ptTop.z + ptBtm.z + ptLeft.z + ptRight.z) / 4;
resultPose.center = { center_x, center_y, center_z };
resultPose.normalDir = { 0, 0, -1.0 };
//旋转回去
for (int i = 0; i < lineNum; i++)
{
//行处理
//调平，去除地面
lineDataRT_vector(scanLines[i], poseR.invRMatrix, -1);
}
double x = resultPose.center.x * poseR.invRMatrix[0] + resultPose.center.y * poseR.invRMatrix[1] + resultPose.center.z * poseR.invRMatrix[2];
double y = resultPose.center.x * poseR.invRMatrix[3] + resultPose.center.y * poseR.invRMatrix[4] + resultPose.center.z * poseR.invRMatrix[5];
double z = resultPose.center.x * poseR.invRMatrix[6] + resultPose.center.y * poseR.invRMatrix[7] + resultPose.center.z * poseR.invRMatrix[8];
resultPose.center = { x, y, z };
x = resultPose.normalDir.x * poseR.invRMatrix[0] + resultPose.normalDir.y * poseR.invRMatrix[1] + resultPose.normalDir.z * poseR.invRMatrix[2];
y = resultPose.normalDir.x * poseR.invRMatrix[3] + resultPose.normalDir.y * poseR.invRMatrix[4] + resultPose.normalDir.z * poseR.invRMatrix[5];
z = resultPose.normalDir.x * poseR.invRMatrix[6] + resultPose.normalDir.y * poseR.invRMatrix[7] + resultPose.normalDir.z * poseR.invRMatrix[8];
resultPose.normalDir = { x, y, z };
return resultPose;
}
void rodAarcFeatueDetection( void rodAarcFeatueDetection(
std::vector< std::vector<SVzNL3DPosition>>& scanLines, std::vector< std::vector<SVzNL3DPosition>>& scanLines,
@ -824,3 +1221,123 @@ void sx_rodPositioning(
return; return;
} }
//筑裕钢结构钢筋焊缝定位
void sx_rebarWeldSeamPositioning(
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
const SSG_planeCalibPara poseCalibPara,
const SSG_cornerParam cornerPara,
const SSG_outlierFilterParam filterParam,
const SSG_treeGrowParam growParam,
const SSX_rodParam rodParam,
std::vector<SSX_weldSeamInfo>& weldSeamInfo,
int* errCode)
{
*errCode = 0;
int lineNum = (int)scanLines.size();
if (lineNum == 0)
{
*errCode = SG_ERR_3D_DATA_NULL;
return;
}
int linePtNum = (int)scanLines[0].size();
//判断数据格式是否为grid。算法只能处理grid数据格式
bool isGridData = true;
for (int line = 0; line < lineNum; line++)
{
if (linePtNum != (int)scanLines[line].size())
{
isGridData = false;
break;
}
}
if (false == isGridData)//数据不是网格格式
{
*errCode = SG_ERR_NOT_GRID_FORMAT;
return;
}
for (int i = 0, i_max = (int)scanLines.size(); i < i_max; i++)
{
if (i == 14)
int kkk = 1;
//行处理
//调平，去除地面
double cuttingZ = -1;
sx_rodPosition_lineDataR(scanLines[i], poseCalibPara.planeCalib, cuttingZ);
}
//寻找水平和垂直方向的钢筋
//生成水平扫描数据
std::vector<std::vector<SVzNL3DPosition>> 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<std::vector<SWD_rodArcFeature>> arcFeatures_v;
rodAarcFeatueDetection(scanLines, cornerPara, filterParam, rodParam.diameter, arcFeatures_v);
//特征生长
std::vector<SWD_rodArcFeatureTree> rodArcTrees_v;
wd_getRodArcFeatureGrowingTrees(arcFeatures_v, rodArcTrees_v, growParam);
//水平方向
std::vector<std::vector<SWD_rodArcFeature>> arcFeatures_h;
rodAarcFeatueDetection(hLines_raw, cornerPara, filterParam, rodParam.diameter, arcFeatures_h);
//特征生长
std::vector<SWD_rodArcFeatureTree> rodArcTrees_h;
wd_getRodArcFeatureGrowingTrees(arcFeatures_h, rodArcTrees_h, growParam);
if ((rodArcTrees_v.size() == 0) && (rodArcTrees_h.size() == 0))
{
*errCode = SG_ERR_NOT_GRID_FORMAT;
return;
}
int objNum_v = (int)rodArcTrees_v.size();
int objNum_h = (int)rodArcTrees_h.size();
for (int line = 0; line < lineNum; line++)
{
for (int j = 0; j < linePtNum; j++)
{
scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
}
}
//置标志，用于debug
for (int i = 0; i < objNum_v; i++)
{
int nodeNum = (int)rodArcTrees_v[i].treeNodes.size();
for (int j = 0; j < nodeNum; j++)
{
int lineIdx = rodArcTrees_v[i].treeNodes[j].lineIdx;
int centerPtIdx = rodArcTrees_v[i].treeNodes[j].peakPtIdx;
for (int m = rodArcTrees_v[i].treeNodes[j].startPtIdx; m <= rodArcTrees_v[i].treeNodes[j].endPtIdx; m++)
scanLines[lineIdx][m].nPointIdx = 1;
scanLines[lineIdx][centerPtIdx].nPointIdx |= 0x10;
}
}
//置标志，用于debug
for (int i = 0; i < objNum_h; i++)
{
int nodeNum = (int)rodArcTrees_h[i].treeNodes.size();
for (int j = 0; j < nodeNum; j++)
{
int ptIdx = rodArcTrees_h[i].treeNodes[j].lineIdx;
int centerLineIdx = rodArcTrees_h[i].treeNodes[j].peakPtIdx;
for (int m = rodArcTrees_h[i].treeNodes[j].startPtIdx; m <= rodArcTrees_h[i].treeNodes[j].endPtIdx; m++)
scanLines[m][ptIdx].nPointIdx |= 2;
scanLines[centerLineIdx][ptIdx].nPointIdx |= 0x20;
}
}
}

37
sourceCode/rodAndBarDetection_Export.h
View File

@ -9,8 +9,13 @@ typedef struct
{ {
SVzNL3DPoint center; //螺杆端部中心点 SVzNL3DPoint center; //螺杆端部中心点
SVzNL3DPoint axialDir; //轴向向量 SVzNL3DPoint axialDir; //轴向向量
double rotateAngle; //-30 - 30¶È }SSX_rodPoseInfo; //
}SSX_hexHeadScrewInfo; //Áù½ÇÍ·ÂݸË
typedef struct
{
SVzNL3DPoint center; //螺杆端部中心点
SVzNL3DPoint normalDir; //法向向量
}SSX_pointPoseInfo; //
typedef struct typedef struct
{ {
@ -27,6 +32,14 @@ typedef struct
SVzNL3DPoint endPt; SVzNL3DPoint endPt;
}SSX_rodPositionInfo; }SSX_rodPositionInfo;
typedef struct
{
SVzNL3DPoint startPt;
SVzNL3DPoint endPt;
SVzNL3DPoint center;
SVzNL3DPoint normalDir; //法向量
}SSX_weldSeamInfo;
//读版本号 //读版本号
SG_APISHARED_EXPORT const char* wd_rodAndBarDetectionVersion(void); SG_APISHARED_EXPORT const char* wd_rodAndBarDetectionVersion(void);
@ -52,9 +65,16 @@ SG_APISHARED_EXPORT void sx_hexHeadScrewMeasure(
const SSG_outlierFilterParam filterParam, const SSG_outlierFilterParam filterParam,
const SSG_treeGrowParam growParam, const SSG_treeGrowParam growParam,
double rodDiameter, double rodDiameter,
std::vector<SSX_hexHeadScrewInfo>& screwInfo, std::vector<SSX_rodPoseInfo>& screwInfo,
int* errCode); int* errCode);
//计算定位盘中心点位姿
SG_APISHARED_EXPORT SSX_pointPoseInfo sx_getLocationPlatePose(
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
const SSG_cornerParam cornerPara,
int* errCode);
//棒材抓取定位
SG_APISHARED_EXPORT void sx_rodPositioning( SG_APISHARED_EXPORT void sx_rodPositioning(
std::vector< std::vector<SVzNL3DPosition>>& scanLines, std::vector< std::vector<SVzNL3DPosition>>& scanLines,
const SSG_planeCalibPara poseCalibPara, const SSG_planeCalibPara poseCalibPara,
@ -63,4 +83,15 @@ SG_APISHARED_EXPORT void sx_rodPositioning(
const SSG_treeGrowParam growParam, const SSG_treeGrowParam growParam,
const SSX_rodParam rodParam, const SSX_rodParam rodParam,
std::vector<SSX_rodPositionInfo>& rodInfo, std::vector<SSX_rodPositionInfo>& rodInfo,
int* errCode);
//筑裕钢结构钢筋焊缝定位
SG_APISHARED_EXPORT void sx_rebarWeldSeamPositioning(
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
const SSG_planeCalibPara poseCalibPara,
const SSG_cornerParam cornerPara,
const SSG_outlierFilterParam filterParam,
const SSG_treeGrowParam growParam,
const SSX_rodParam rodParam,
std::vector<SSX_weldSeamInfo>& weldSeamInfo,
int* errCode); int* errCode);