diff --git a/bagPositioning/bagPositioning.vcxproj b/bagPositioning/bagPositioning.vcxproj
index 0f1d019..91190a6 100644
--- a/bagPositioning/bagPositioning.vcxproj
+++ b/bagPositioning/bagPositioning.vcxproj
@@ -42,13 +42,13 @@
DynamicLibrary
true
- v142
+ v143
Unicode
DynamicLibrary
false
- v142
+ v143
true
Unicode
diff --git a/rodAndBarDetection_test/rodAndBarDetection_test.cpp b/rodAndBarDetection_test/rodAndBarDetection_test.cpp
index 9e9a4de..a067a68 100644
--- a/rodAndBarDetection_test/rodAndBarDetection_test.cpp
+++ b/rodAndBarDetection_test/rodAndBarDetection_test.cpp
@@ -208,7 +208,7 @@ SSG_planeCalibPara _readCalibPara(char* fileName)
}
-void _outputChanneltInfo(char* fileName, std::vector& screwInfo)
+void _outputChanneltInfo(char* fileName, std::vector& screwInfo)
{
std::ofstream sw(fileName);
@@ -216,14 +216,25 @@ void _outputChanneltInfo(char* fileName, std::vector& scre
int objNum = (int)screwInfo.size();
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,
- 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.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& rodInfo)
{
std::ofstream sw(fileName);
@@ -241,10 +252,26 @@ void _outputRodtInfo(char* fileName, std::vector& rodInfo)
sw.close();
}
+void _outputWeldSeamInfo(char* fileName, std::vector& 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(
char* fileName,
std::vector>& scanLines,
- std::vector& screwInfo
+ std::vector& screwInfo
)
{
int lineNum = (int)scanLines.size();
@@ -377,6 +404,112 @@ void _outputRGBDScan_RGBD(
sw.close();
}
+void _outputRGBDScan_RGBD_centerPose(
+ char* fileName,
+ std::vector>& 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(
char* fileName,
std::vector>& scanLines,
@@ -511,6 +644,139 @@ void _outputRGBDScan_RGBD_rodInfo(
}
sw.close();
}
+
+void _outputRGBDScan_RGBD_weldSeam(
+ char* fileName,
+ std::vector>& scanLines,
+ std::vector& 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
void screwTest(void)
{
@@ -566,7 +832,7 @@ void screwTest(void)
bool isHorizonScan = true; //true:激光线平行槽道；false:激光线垂直槽道
int errCode = 0;
- std::vector screwInfo;
+ std::vector screwInfo;
sx_hexHeadScrewMeasure(
scanLines,
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> 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
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> 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 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()
{
-#if 0 //螺杆定位测试
+#if 1 //螺杆定位测试
+#if 0
screwTest();
+#else
+ locatingPlateTest();
+#endif
#else //棒材抓取定位测试
+#if 0
rodPositionTest();
+#else
+ rodWeldSeamPosition_test();
+#endif
#endif
return 0;
}
diff --git a/sourceCode/SG_baseAlgo_Export.h b/sourceCode/SG_baseAlgo_Export.h
index 4cadef3..39fcf0d 100644
--- a/sourceCode/SG_baseAlgo_Export.h
+++ b/sourceCode/SG_baseAlgo_Export.h
@@ -139,6 +139,14 @@ SG_APISHARED_EXPORT void wd_getLineCornerFeature_PSM(
const SSG_cornerParam cornerPara,
SSG_lineFeature* line_features);
+SG_APISHARED_EXPORT void wd_lineDataSegment_zDist(
+ std::vector< SVzNL3DPosition>& lineData,
+ std::vector< SVzNL3DPosition>& vldPts,
+ std::vector& segs,
+ std::vector& backIndexing,
+ const SSG_cornerParam cornerPara
+);
+
SG_APISHARED_EXPORT void wd_computeDirAngle_wholeLine(
std::vector< SVzNL3DPosition>& line_data,
const SSG_cornerParam cornerPara,
@@ -643,8 +651,8 @@ SG_APISHARED_EXPORT void vzCaculateLaserPlane(
SG_APISHARED_EXPORT Plane robustFitPlane(
const std::vector& points,
- RobustType type = TUKEY,
- double delta = 0.5, // ֵ>ֵΪȺ㣨mm
+ RobustType type = HUBER,
+ double delta = 1.0, // ֵ>ֵΪȺ㣨mm
int maxIter = 20, //
double convergeThresh = 1e-5 // ֵƽ仯㹻Сͣ
);
@@ -740,6 +748,7 @@ SG_APISHARED_EXPORT void wd_pointClustering_speedUp(
std::vector< SVzNL3DPosition>& pts,
int lineNum, int ptSize, int clusterCheckWin, //
double clusterDist,
+ int distType, //0 - 2d distance; 1- 3d distance
std::vector>& objClusters //result
);
diff --git a/sourceCode/SG_clustering.cpp b/sourceCode/SG_clustering.cpp
index e562a37..d02214e 100644
--- a/sourceCode/SG_clustering.cpp
+++ b/sourceCode/SG_clustering.cpp
@@ -41,6 +41,7 @@ void _seedClustering_speedUp(
std::vector< SVzNL3DPosition>& pts,
std::vector& flags,
double clusterDist,
+ int distType, //0 - 2d distance; 1- 3d distance
std::vector>& indexing2D,
int lineNum, int ptSize, int clusterCheckWin) //
{
@@ -66,7 +67,11 @@ void _seedClustering_speedUp(
continue;
if (flags[backIdx] < 0)
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)
{
a_cluster.push_back(pts[backIdx]);
@@ -129,6 +134,7 @@ void wd_pointClustering_speedUp(
std::vector< SVzNL3DPosition>& pts,
int lineNum, int linePtSize, int clusterCheckWin, //
double clusterDist,
+ int distType, //0 - 2d distance; 1- 3d distance
std::vector>& objClusters //result
)
{
@@ -170,9 +176,11 @@ void wd_pointClustering_speedUp(
pts,
flags,
clusterDist,
+ distType,
indexing2D,
lineNum, linePtSize, clusterCheckWin);
- objClusters.push_back(a_cluster); //һ
+ if(a_cluster.size() >= 5)
+ objClusters.push_back(a_cluster); //һ
}
idx++;
}
diff --git a/sourceCode/SG_lineFeature.cpp b/sourceCode/SG_lineFeature.cpp
index 267013e..b8abfa0 100644
--- a/sourceCode/SG_lineFeature.cpp
+++ b/sourceCode/SG_lineFeature.cpp
@@ -3825,7 +3825,7 @@ bool compareByPtIdx(const SSG_basicFeature1D& a, const SSG_basicFeature1D& b) {
}
//ZԷֶ
-void _lineDataSegment_zDist(
+void wd_lineDataSegment_zDist(
std::vector< SVzNL3DPosition>& lineData,
std::vector< SVzNL3DPosition>& vldPts,
std::vector& segs,
@@ -4089,7 +4089,7 @@ void wd_getRingArcFeature(
backIndexing.resize(dataSize);
//zԷֶ
- _lineDataSegment_zDist(lineData, vldPts, segs, backIndexing, cornerPara);
+ wd_lineDataSegment_zDist(lineData, vldPts, segs, backIndexing, cornerPara);
//ǰǺͺ
std::vector< SSG_pntDirAngle> ptDirAngles;
_computeDirAngle_perSeg(vldPts, segs, cornerPara, ptDirAngles);
diff --git a/sourceCode/rodAndBarDetection.cpp b/sourceCode/rodAndBarDetection.cpp
index 9550bef..adb7c48 100644
--- a/sourceCode/rodAndBarDetection.cpp
+++ b/sourceCode/rodAndBarDetection.cpp
@@ -8,7 +8,8 @@
//version 1.0.0 : base version release to customer
//version 1.1.0 : ˵ƽͰĶλ
//version 1.1.1 : ʼͻİ汾
-std::string m_strVersion = "1.1.1";
+//version 1.2.0 : ݸ˲˶λIJ
+std::string m_strVersion = "1.2.0";
const char* wd_rodAndBarDetectionVersion(void)
{
return m_strVersion.c_str();
@@ -204,6 +205,7 @@ SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, double matrix3d[9])
return _r_pt;
}
+//ݸ˶˲ĵλ
void sx_hexHeadScrewMeasure(
std::vector< std::vector>& scanLines,
bool isHorizonScan, //true:ƽв۵false:ߴֱ۵
@@ -211,7 +213,7 @@ SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, double matrix3d[9])
const SSG_outlierFilterParam filterParam,
const SSG_treeGrowParam growParam,
double rodDiameter,
- std::vector& screwInfo,
+ std::vector& screwInfo,
int* errCode)
{
*errCode = 0;
@@ -429,10 +431,9 @@ SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, double matrix3d[9])
//תԭϵ
SVzNL3DPoint surfaceCenter = _ptRotate(projectionCenter, rotatePara.invRMatrix);
//RodϢ
- SSX_hexHeadScrewInfo a_rod;
+ SSX_rodPoseInfo a_rod;
a_rod.center = surfaceCenter;
a_rod.axialDir = P1_dir;
- a_rod.rotateAngle = 0;
screwInfo.push_back(a_rod);
}
if (true == isHorizonScan)
@@ -452,6 +453,402 @@ SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, double matrix3d[9])
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>& 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> 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> 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 segs;
+ std::vector 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 segs;
+ std::vector 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> objClusters; //result
+ wd_pointClustering_speedUp(
+ endingPts,
+ lineNum, linePtNum, clusterCheckWin, //
+ clusterDist,
+ distType,
+ objClusters //result
+ );
+
+ //жϱ
+ std::vector 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 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 out_inliers;
+ //Plane res = ransacFitPlane( Points3ds, out_inliers );
+ Plane res = robustFitPlane(Points3ds);
+
+ //std::vector 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 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(
std::vector< std::vector>& scanLines,
@@ -824,3 +1221,123 @@ void sx_rodPositioning(
return;
}
+//ԣֽṹֽ춨λ
+void sx_rebarWeldSeamPositioning(
+ std::vector< std::vector>& scanLines,
+ const SSG_planeCalibPara poseCalibPara,
+ const SSG_cornerParam cornerPara,
+ const SSG_outlierFilterParam filterParam,
+ const SSG_treeGrowParam growParam,
+ const SSX_rodParam rodParam,
+ std::vector& 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> 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> arcFeatures_v;
+ rodAarcFeatueDetection(scanLines, cornerPara, filterParam, rodParam.diameter, arcFeatures_v);
+ //
+ std::vector rodArcTrees_v;
+ wd_getRodArcFeatureGrowingTrees(arcFeatures_v, rodArcTrees_v, growParam);
+
+ //ˮƽ
+ std::vector> arcFeatures_h;
+ rodAarcFeatueDetection(hLines_raw, cornerPara, filterParam, rodParam.diameter, arcFeatures_h);
+ //
+ std::vector 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;
+ }
+ }
+}
diff --git a/sourceCode/rodAndBarDetection_Export.h b/sourceCode/rodAndBarDetection_Export.h
index 1c496bb..a3cadb6 100644
--- a/sourceCode/rodAndBarDetection_Export.h
+++ b/sourceCode/rodAndBarDetection_Export.h
@@ -9,8 +9,13 @@ typedef struct
{
SVzNL3DPoint center; //ݸ˶˲ĵ
SVzNL3DPoint axialDir; //
- double rotateAngle; //-30 - 30
-}SSX_hexHeadScrewInfo; //ͷݸ
+}SSX_rodPoseInfo; //
+
+typedef struct
+{
+ SVzNL3DPoint center; //ݸ˶˲ĵ
+ SVzNL3DPoint normalDir; //
+}SSX_pointPoseInfo; //
typedef struct
{
@@ -27,6 +32,14 @@ typedef struct
SVzNL3DPoint endPt;
}SSX_rodPositionInfo;
+typedef struct
+{
+ SVzNL3DPoint startPt;
+ SVzNL3DPoint endPt;
+ SVzNL3DPoint center;
+ SVzNL3DPoint normalDir; //
+}SSX_weldSeamInfo;
+
//汾
SG_APISHARED_EXPORT const char* wd_rodAndBarDetectionVersion(void);
@@ -52,9 +65,16 @@ SG_APISHARED_EXPORT void sx_hexHeadScrewMeasure(
const SSG_outlierFilterParam filterParam,
const SSG_treeGrowParam growParam,
double rodDiameter,
- std::vector& screwInfo,
+ std::vector& screwInfo,
int* errCode);
+//㶨λĵλ
+SG_APISHARED_EXPORT SSX_pointPoseInfo sx_getLocationPlatePose(
+ std::vector< std::vector>& scanLines,
+ const SSG_cornerParam cornerPara,
+ int* errCode);
+
+//ץȡλ
SG_APISHARED_EXPORT void sx_rodPositioning(
std::vector< std::vector>& scanLines,
const SSG_planeCalibPara poseCalibPara,
@@ -63,4 +83,15 @@ SG_APISHARED_EXPORT void sx_rodPositioning(
const SSG_treeGrowParam growParam,
const SSX_rodParam rodParam,
std::vector& rodInfo,
+ int* errCode);
+
+//ԣֽṹֽ춨λ
+SG_APISHARED_EXPORT void sx_rebarWeldSeamPositioning(
+ std::vector< std::vector>& scanLines,
+ const SSG_planeCalibPara poseCalibPara,
+ const SSG_cornerParam cornerPara,
+ const SSG_outlierFilterParam filterParam,
+ const SSG_treeGrowParam growParam,
+ const SSX_rodParam rodParam,
+ std::vector& weldSeamInfo,
int* errCode);
\ No newline at end of file