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

更工件算法

Browse Source
This commit is contained in:
jerryzeng 2025-11-08 09:57:14 +08:00
parent 908145f028
commit ceb2f76484
14 changed files with 1815 additions and 336 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
BQ_workpieceCornerExtract_test/BQ_workpieceCornerExtract_test.cpp
View File

@ -501,7 +501,7 @@ void _outputRGBDScanLapWeld_RGBD(
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
} }
if (debugData) if ((debugData) && (workpieceCorner.workpieceType > 0))
{ {
int linePtNum = debugData[0].edge_size + debugData[0].edgeLink1_size + debugData[0].edgeLink2_size; 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[1].edge_size + debugData[1].edgeLink1_size + debugData[1].edgeLink2_size;
@ -606,15 +606,20 @@ void _outputRGBDScanLapWeld_RGBD(
#define CONVERT_TO_GRID 0 #define CONVERT_TO_GRID 0
#define TEST_COMPUTE_CALIB_PARA 0 #define TEST_COMPUTE_CALIB_PARA 0
#define TEST_COMPUTE_CORNER 1 #define TEST_COMPUTE_CORNER 1
#define TEST_GROUP 1 #define TEST_GROUP 6
int main() int main()
{ {
const char* dataPath[TEST_GROUP] = { const char* dataPath[TEST_GROUP] = {
"F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\" //0 "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\", //0
"F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\工件点云\\角度1\\", //1
"F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\工件点云\\角度2\\", //2
"F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\工件点云\\角度3\\", //3
"F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\工件点云\\角度4\\", //4
"F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\工件点云\\", //5
}; };
SVzNLRange fileIdx[TEST_GROUP] = { SVzNLRange fileIdx[TEST_GROUP] = {
{1,3} {1,3}, {6,8}, {6,8}, {6,8}, {6,8}, {9,12}
}; };
#if CONVERT_TO_GRID #if CONVERT_TO_GRID
@ -641,7 +646,7 @@ int main()
#if TEST_COMPUTE_CALIB_PARA #if TEST_COMPUTE_CALIB_PARA
char _calib_datafile[256]; char _calib_datafile[256];
sprintf_s(_calib_datafile, "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\scanData_1.txt"); sprintf_s(_calib_datafile, "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\工件点云\\9_LazerData_Hi229229.txt");
int lineNum = 0; int lineNum = 0;
float lineV = 0.0f; float lineV = 0.0f;
int dataCalib = 0; int dataCalib = 0;
@ -677,10 +682,10 @@ int main()
} }
// //
char calibFile[250]; char calibFile[250];
sprintf_s(calibFile, "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\ground_calib_para.txt"); sprintf_s(calibFile, "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\工件点云\\ground_calib_para.txt");
_outputCalibPara(calibFile, calibPara); _outputCalibPara(calibFile, calibPara);
char _out_file[256]; char _out_file[256];
sprintf_s(_out_file, "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\scanData_ground_1_calib.txt"); sprintf_s(_out_file, "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\工件点云\\scanData_ground_1_calib.txt");
int headNullLines = 0; int headNullLines = 0;
_outputScanDataFile_vector(_out_file, scanData, false, &headNullLines); _outputScanDataFile_vector(_out_file, scanData, false, &headNullLines);
printf("%s: calib done!\n", _calib_datafile); printf("%s: calib done!\n", _calib_datafile);
@ -688,7 +693,10 @@ int main()
#endif #endif
#if TEST_COMPUTE_CORNER #if TEST_COMPUTE_CORNER
for (int grp = 0; grp <= 0; grp++) const char* ver = wd_BQWorkpieceCornerVersion();
printf("ver:%s\n", ver);
for (int grp = 1; grp <= 5; grp++)
{ {
SSG_planeCalibPara poseCalibPara; SSG_planeCalibPara poseCalibPara;
//初始化成单位阵 //初始化成单位阵
@ -710,12 +718,40 @@ int main()
sprintf_s(calibFile, "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\ground_calib_para.txt"); sprintf_s(calibFile, "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\ground_calib_para.txt");
poseCalibPara = _readCalibPara(calibFile); poseCalibPara = _readCalibPara(calibFile);
} }
else if (grp == 1)
{
sprintf_s(calibFile, "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\工件点云\\角度1\\ground_calib_para.txt");
poseCalibPara = _readCalibPara(calibFile);
}
else if (grp == 2)
{
sprintf_s(calibFile, "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\工件点云\\角度2\\ground_calib_para.txt");
poseCalibPara = _readCalibPara(calibFile);
}
else if (grp == 3)
{
sprintf_s(calibFile, "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\工件点云\\角度3\\ground_calib_para.txt");
poseCalibPara = _readCalibPara(calibFile);
}
else if (grp == 4)
{
sprintf_s(calibFile, "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\工件点云\\角度4\\ground_calib_para.txt");
poseCalibPara = _readCalibPara(calibFile);
}
else if (grp == 5)
{
sprintf_s(calibFile, "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\工件点云\\ground_calib_para.txt");
poseCalibPara = _readCalibPara(calibFile);
}
for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
{ {
//fidx =1; //fidx =1;
char _scan_file[256]; char _scan_file[256];
if(0 == grp)
sprintf_s(_scan_file, "%sscanData_%d_grid.txt", dataPath[grp], fidx); sprintf_s(_scan_file, "%sscanData_%d_grid.txt", dataPath[grp], fidx);
else
sprintf_s(_scan_file, "%s%d_LazerData_Hi229229.txt", dataPath[grp], fidx);
std::vector<std::vector< SVzNL3DPosition>> scanLines; std::vector<std::vector< SVzNL3DPosition>> scanLines;
vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines); vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines);

168
QRcode3Ddetection_test/QRcode3Ddetection_test.cpp
View File

@ -73,6 +73,95 @@ void vzReadLaserScanPointFromFile_plyTxt(const char* fileName, std::vector< SVzN
return; return;
} }
typedef struct
{
int x;
double y;
double z;
} WD_Encode3DPoint;
void vzReadLaserScanPointFromFile_encodePlyTxt(const char* fileName, std::vector< WD_Encode3DPoint>& scanData)
{
std::ifstream inputFile(fileName);
std::string linedata;
if (inputFile.is_open() == false)
return;
while (std::getline(inputFile, linedata))
{
if (linedata.empty())
continue;
int X;
double Y, Z, tmp;
sscanf_s(linedata.c_str(), "%d,%lf,%lf", &X, &Y, &Z);
WD_Encode3DPoint a_pt;
a_pt.x = X;
a_pt.y = Y;
a_pt.z = Z;
scanData.push_back(a_pt);
}
return;
}
void convertEncodePlyToVzData(std::vector< WD_Encode3DPoint>& scanData, std::vector<std::vector<SVzNL3DPosition>>& scanLines, double stepping, bool toGrid)
{
int size = (int)scanData.size();
int preEnc = -1;
std::vector<SVzNL3DPosition> a_line;
int vldNum = 0;
double lineX = 0;
for (int i = 0; i < size; i++)
{
WD_Encode3DPoint a_ply = scanData[i];
if (a_ply.x != preEnc) //new line
{
if (vldNum > 0)
{
scanLines.push_back(a_line);
a_line.clear();
}
if (preEnc < 0)
lineX = 0;
else
lineX += (double)(a_ply.x - preEnc) * stepping;
preEnc = a_ply.x;
vldNum = 0;
}
SVzNL3DPosition a_pt;
a_pt.nPointIdx = vldNum;
bool zeroPt = true;
if ((a_ply.z > 29.0) && (a_ply.z < 34.0))
{
a_pt.pt3D.x = lineX;
a_pt.pt3D.y = a_ply.y;
a_pt.pt3D.z = a_ply.z;
zeroPt = false;
}
else
{
a_pt.pt3D.x = 0;
a_pt.pt3D.y = 0;
a_pt.pt3D.z = 0;
}
if (toGrid == true)
{
a_line.push_back(a_pt);
vldNum++;
}
else
{
if (false == zeroPt)
{
a_line.push_back(a_pt);
vldNum++;
}
}
}
}
void _outputScanDataFile_vector(char* fileName, std::vector<std::vector<SVzNL3DPosition>>& scanLines, bool removeZeros, int* headNullLines) void _outputScanDataFile_vector(char* fileName, std::vector<std::vector<SVzNL3DPosition>>& scanLines, bool removeZeros, int* headNullLines)
{ {
std::ofstream sw(fileName); std::ofstream sw(fileName);
@ -133,6 +222,46 @@ void _outputScanDataFile_vector(char* fileName, std::vector<std::vector<SVzNL3DP
sw.close(); sw.close();
} }
void _outputScanPlyTxtFile_vector(char* fileName, std::vector<std::vector<SVzNL3DPosition>>& scanLines, bool removeZeros, int* headNullLines)
{
std::ofstream sw(fileName);
int lineNum = scanLines.size();
if (lineNum == 0)
return;
int null_lines = 0;
bool counterNull = true;
for (int line = 0; line < lineNum; line++)
{
int linePtNum = scanLines[line].size();
if (linePtNum == 0)
continue;
bool isNull = true;
for (int i = 0; i < linePtNum; i++)
{
SVzNL3DPoint* pt3D = &scanLines[line][i].pt3D;
if ((pt3D->z > 1e-4) && (isNull == true))
isNull = false;
if ((true == removeZeros) && (pt3D->z < 1e-4))
continue;
float x = (float)pt3D->x;
float y = (float)pt3D->y;
float z = (float)pt3D->z;
sw << x << "," << y << "," << z<< std::endl;
}
if (true == counterNull)
{
if (true == isNull)
null_lines++;
else
counterNull = false;
}
}
*headNullLines = null_lines;
sw.close();
}
void _outputRGBDScanDataFile_RGBD( void _outputRGBDScanDataFile_RGBD(
char* fileName, char* fileName,
std::vector<std::vector<SVzNL3DPosition>>& scanLines, std::vector<std::vector<SVzNL3DPosition>>& scanLines,
@ -562,20 +691,53 @@ void _getRoiClouds(
return; return;
} }
#define CONVERT_TO_GRID 1
#define TEST_COMPUTE_CALIB_PARA 0 #define TEST_COMPUTE_CALIB_PARA 0
#define TEST_COMPUTE_QRCODE_IMG 1 #define TEST_COMPUTE_QRCODE_IMG 0
#define TEST_GROUP 2 #define TEST_GROUP 2
int main() int main()
{ {
const char* dataPath[TEST_GROUP] = { const char* dataPath[TEST_GROUP] = {
"F:\\ShangGu\\项目\\工件端部圆点二维码\\", //0 "F:\\ShangGu\\项目\\国铭铸管\\二维码\\", //0
"F:\\ShangGu\\项目\\工件端部圆点二维码\\字符\\" //1 "F:\\ShangGu\\项目\\国铭铸管\\字符\\" //1
}; };
SVzNLRange fileIdx[TEST_GROUP] = { SVzNLRange fileIdx[TEST_GROUP] = {
{3,3}, {1,8} {3,3}, {1,8}
}; };
#if CONVERT_TO_GRID
int convertGrp = 1;
for (int fidx = fileIdx[convertGrp].nMin; fidx <= fileIdx[convertGrp].nMax; fidx++)
{
char _scan_file[256];
sprintf_s(_scan_file, "%sdata%d.txt", dataPath[convertGrp], fidx);
std::vector< WD_Encode3DPoint> scanPly;
vzReadLaserScanPointFromFile_encodePlyTxt(_scan_file, scanPly);
double stepping = 0.0625;
std::vector<std::vector< SVzNL3DPosition>> scanData;
std::vector<std::vector< SVzNL3DPosition>> gridScanData;
bool toGrid = false;
convertEncodePlyToVzData(scanPly, scanData, stepping, toGrid);
toGrid = true;
convertEncodePlyToVzData(scanPly, gridScanData, stepping, toGrid);
//将数据恢复为按扫描线存储格式
sprintf_s(_scan_file, "%sscanDataPlyTxt_%d.txt", dataPath[convertGrp], fidx);
int headNullLines = 0;
_outputScanPlyTxtFile_vector(_scan_file, scanData, false, &headNullLines);
#if 0
sprintf_s(_scan_file, "%svzScanData_%d.txt", dataPath[convertGrp], fidx);
int headNullLines = 0;
_outputScanDataFile_vector(_scan_file, scanData, false, &headNullLines);
sprintf_s(_scan_file, "%svzScanData_grid_%d.txt", dataPath[convertGrp], fidx);
_outputScanDataFile_vector(_scan_file, gridScanData, false, &headNullLines);
printf("%s: head null lines = %d\n", _scan_file, headNullLines);
#endif
}
#endif
#if TEST_COMPUTE_CALIB_PARA #if TEST_COMPUTE_CALIB_PARA
char _calib_datafile[256]; char _calib_datafile[256];
sprintf_s(_calib_datafile, "F:\\ShangGu\\项目\\工件端部圆点二维码\\LaserLine3_grid.txt"); sprintf_s(_calib_datafile, "F:\\ShangGu\\项目\\工件端部圆点二维码\\LaserLine3_grid.txt");

26
SG_Algorithm.sln
View File

@ -118,6 +118,16 @@ Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "BQ_workpieceCornerExtract_t
{AD8415B7-A745-4184-87B8-95619E5066D6} = {AD8415B7-A745-4184-87B8-95619E5066D6} {AD8415B7-A745-4184-87B8-95619E5066D6} = {AD8415B7-A745-4184-87B8-95619E5066D6}
EndProjectSection EndProjectSection
EndProject EndProject
Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "particleSizeMeasurement_test", "particleSizeMeasurement_test\particleSizeMeasurement_test.vcxproj", "{0F4995A6-3978-4EF6-87AD-CC15EC9B007B}"
ProjectSection(ProjectDependencies) = postProject
{8FD0F574-61C6-4094-8521-33AEDDB44797} = {8FD0F574-61C6-4094-8521-33AEDDB44797}
EndProjectSection
EndProject
Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "particleSizeMeasurement", "particleSizeMeasurement\particleSizeMeasurement.vcxproj", "{8FD0F574-61C6-4094-8521-33AEDDB44797}"
ProjectSection(ProjectDependencies) = postProject
{95DC3F1A-902A-490E-BD3B-B10463CF0EBD} = {95DC3F1A-902A-490E-BD3B-B10463CF0EBD}
EndProjectSection
EndProject
Global Global
GlobalSection(SolutionConfigurationPlatforms) = preSolution GlobalSection(SolutionConfigurationPlatforms) = preSolution
Debug|x64 = Debug|x64 Debug|x64 = Debug|x64
@ -310,6 +320,22 @@ Global
{CF563709-0402-447E-BFCC-7701CC90D0AF}.Release|x64.Build.0 = Release|x64 {CF563709-0402-447E-BFCC-7701CC90D0AF}.Release|x64.Build.0 = Release|x64
{CF563709-0402-447E-BFCC-7701CC90D0AF}.Release|x86.ActiveCfg = Release|Win32 {CF563709-0402-447E-BFCC-7701CC90D0AF}.Release|x86.ActiveCfg = Release|Win32
{CF563709-0402-447E-BFCC-7701CC90D0AF}.Release|x86.Build.0 = Release|Win32 {CF563709-0402-447E-BFCC-7701CC90D0AF}.Release|x86.Build.0 = Release|Win32
{0F4995A6-3978-4EF6-87AD-CC15EC9B007B}.Debug|x64.ActiveCfg = Debug|x64
{0F4995A6-3978-4EF6-87AD-CC15EC9B007B}.Debug|x64.Build.0 = Debug|x64
{0F4995A6-3978-4EF6-87AD-CC15EC9B007B}.Debug|x86.ActiveCfg = Debug|Win32
{0F4995A6-3978-4EF6-87AD-CC15EC9B007B}.Debug|x86.Build.0 = Debug|Win32
{0F4995A6-3978-4EF6-87AD-CC15EC9B007B}.Release|x64.ActiveCfg = Release|x64
{0F4995A6-3978-4EF6-87AD-CC15EC9B007B}.Release|x64.Build.0 = Release|x64
{0F4995A6-3978-4EF6-87AD-CC15EC9B007B}.Release|x86.ActiveCfg = Release|Win32
{0F4995A6-3978-4EF6-87AD-CC15EC9B007B}.Release|x86.Build.0 = Release|Win32
{8FD0F574-61C6-4094-8521-33AEDDB44797}.Debug|x64.ActiveCfg = Debug|x64
{8FD0F574-61C6-4094-8521-33AEDDB44797}.Debug|x64.Build.0 = Debug|x64
{8FD0F574-61C6-4094-8521-33AEDDB44797}.Debug|x86.ActiveCfg = Debug|Win32
{8FD0F574-61C6-4094-8521-33AEDDB44797}.Debug|x86.Build.0 = Debug|Win32
{8FD0F574-61C6-4094-8521-33AEDDB44797}.Release|x64.ActiveCfg = Release|x64
{8FD0F574-61C6-4094-8521-33AEDDB44797}.Release|x64.Build.0 = Release|x64
{8FD0F574-61C6-4094-8521-33AEDDB44797}.Release|x86.ActiveCfg = Release|Win32
{8FD0F574-61C6-4094-8521-33AEDDB44797}.Release|x86.Build.0 = Release|Win32
EndGlobalSection EndGlobalSection
GlobalSection(SolutionProperties) = preSolution GlobalSection(SolutionProperties) = preSolution
HideSolutionNode = FALSE HideSolutionNode = FALSE

6
bagPositioning_test/bagPositioning_test.cpp
View File

@ -2871,8 +2871,8 @@ int main()
else if (grp == 19) //纸箱拆垛数据\vizum数据 else if (grp == 19) //纸箱拆垛数据\vizum数据
{ {
algoParam.bagParam.bagL = 500;// 500; // 420; //袋子长65cm algoParam.bagParam.bagL = 500;// 500; // 420; //袋子长65cm
algoParam.bagParam.bagW = 320; // 420; // 320; //袋子宽40cm algoParam.bagParam.bagW = 350; // 420; // 320; //袋子宽40cm
algoParam.bagParam.bagH = 70; //袋子高16cm algoParam.bagParam.bagH = 80; //袋子高16cm
algoParam.growParam.maxLineSkipNum = 5; algoParam.growParam.maxLineSkipNum = 5;
algoParam.growParam.yDeviation_max = 20.0; algoParam.growParam.yDeviation_max = 20.0;
algoParam.growParam.maxSkipDistance = 20.0; algoParam.growParam.maxSkipDistance = 20.0;
@ -3039,7 +3039,7 @@ int main()
//fidx = 193; //fidx = 193;
if (grp < TEST_TOP_VIEW_GROUP) //正面抓取 if (grp < TEST_TOP_VIEW_GROUP) //正面抓取
{ {
algoParam.supportRotate = 1; //支持相机与对象之间有一个旋转角度（小于30度） algoParam.supportRotate = 0; //支持相机与对象之间有一个旋转角度（小于30度）
int lineNum = 0; int lineNum = 0;
float lineV = 0.0f; float lineV = 0.0f;

1
baseAlgorithm/baseAlgorithm.vcxproj
View File

@ -173,6 +173,7 @@
<ClCompile Include="..\sourceCode\SG_featureGrow.cpp" /> <ClCompile Include="..\sourceCode\SG_featureGrow.cpp" />
<ClCompile Include="..\sourceCode\SG_lineFeature.cpp" /> <ClCompile Include="..\sourceCode\SG_lineFeature.cpp" />
<ClCompile Include="..\sourceCode\SG_regionGrow.cpp" /> <ClCompile Include="..\sourceCode\SG_regionGrow.cpp" />
<ClCompile Include="..\sourceCode\WD_watershed.cpp" />
</ItemGroup> </ItemGroup>
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" /> <Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />
<ImportGroup Label="ExtensionTargets"> <ImportGroup Label="ExtensionTargets">

470
sourceCode/BQ_workpieceCornerExtraction.cpp
View File

@ -5,6 +5,12 @@
#include <opencv2/opencv.hpp> #include <opencv2/opencv.hpp>
#include <limits> #include <limits>
std::string m_strVersion = "1.1.0";
const char* wd_BQWorkpieceCornerVersion(void)
{
return m_strVersion.c_str();
}
//计算一个平面调平参数。 //计算一个平面调平参数。
//数据输入中可以有一个地平面和参考调平平面，以最高的平面进行调平 //数据输入中可以有一个地平面和参考调平平面，以最高的平面进行调平
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数 //旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
@ -154,6 +160,27 @@ typedef struct
SVzNL3DPoint edge_link2_ends[2]; SVzNL3DPoint edge_link2_ends[2];
}SSX_featureContour; }SSX_featureContour;
typedef struct
{
int lineIdx;
int ptIdx;
double R;
double angle;
double x;
double y;
double z;
}SWD_polarPt;
//逆时针旋转时 θ > 0 ；顺时针旋转时 θ < 0
cv::Point2f _rotate2D(cv::Point2f pt, double sinTheta, double cosTheta)
{
return (cv::Point2f((float)(pt.x * cosTheta - pt.y * sinTheta), (float)(pt.x * sinTheta + pt.y * cosTheta)));
}
bool compareByAngle(const SWD_polarPt& a, const SWD_polarPt& b) {
return a.angle < b.angle;
}
SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners( SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
std::vector< std::vector<SVzNL3DPosition>>& scanLines, std::vector< std::vector<SVzNL3DPosition>>& scanLines,
const SSG_cornerParam cornerPara, const SSG_cornerParam cornerPara,
@ -179,6 +206,440 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
int linePtNum = (int)scanLines[0].size(); int linePtNum = (int)scanLines[0].size();
bool isGridData = true; bool isGridData = true;
SSX_featureContour region[4];
//自适应各种旋转角度
{
//垂直跳变特征提取
std::vector<std::vector<SSG_basicFeature1D>> jumpFeatures_v_raw;
for (int line = 0; line < lineNum; line++)
{
if (line == 202)
int kkk = 1;
std::vector<SVzNL3DPosition>& lineData = scanLines[line];
if (linePtNum != (int)lineData.size())
isGridData = false;
//滤波，滤除异常点
sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam);
std::vector<SSG_basicFeature1D> line_features;
int dataSize = (int)lineData.size();
sg_getLineCornerFeature_BQ(
&lineData[0],
dataSize,
line,
groundCalibPara.planeHeight,
cornerPara, //scale通常取bagH的1/4
line_features);
jumpFeatures_v_raw.push_back(line_features);
}
if (false == isGridData)//数据不是网格格式
{
*errCode = SG_ERR_NOT_GRID_FORMAT;
return workpieceCorners;
}
//生成水平扫描
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<SSG_basicFeature1D>> jumpFeatures_h_raw;
int lineNum_h_raw = (int)hLines_raw.size();
for (int line = 0; line < lineNum_h_raw; line++)
{
if (line == 416)
int kkk = 1;
std::vector<SVzNL3DPosition>& lineData = hLines_raw[line];
//滤波，滤除异常点
int ptNum = (int)lineData.size();
sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], ptNum, filterParam);
std::vector<SSG_basicFeature1D> line_features;
int dataSize = (int)lineData.size();
sg_getLineCornerFeature_BQ(
&hLines_raw[line][0],
dataSize,
line,
groundCalibPara.planeHeight,
cornerPara, //scale通常取bagH的1/4
line_features);
jumpFeatures_h_raw.push_back(line_features);
}
//特征生长，用于滤除噪点
//垂直方向特征生长（激光线方向）
std::vector<SSG_featureTree> v_trees;
for (int line = 0; line < lineNum; line++)
{
bool isLastLine = false;
if (line == lineNum - 1)
isLastLine = true;
std::vector<SSG_basicFeature1D>& a_lineJumpFeature = jumpFeatures_v_raw[line];
if (a_lineJumpFeature.size() > 0)
int kkk = 1;
if (line == 202)
int kkk = 1;
sg_lineFeaturesGrowing(
line,
isLastLine,
a_lineJumpFeature,
v_trees,
growParam);
}
//水平方向特征生长（扫描运动方向）
std::vector<SSG_featureTree> h_trees;
for (int line = 0; line < lineNum_h_raw; line++)
{
if (line == 650)
int kkk = 1;
bool isLastLine = false;
if (line == lineNum_h_raw - 1)
isLastLine = true;
std::vector<SSG_basicFeature1D>& a_lineJumpFeature = jumpFeatures_h_raw[line];
sg_lineFeaturesGrowing(
line,
isLastLine,
a_lineJumpFeature,
h_trees,
growParam);
}
std::vector<SWD_polarPt> polarPoints;
#if 0
for (int line = 0; line < lineNum; line++)
{
std::vector<SSG_basicFeature1D>& a_lineJumpFeature = jumpFeatures_v_raw[line];
for (int pi = 0, pi_max = (int)a_lineJumpFeature.size(); pi < pi_max; pi++)
{
int lineIdx = a_lineJumpFeature[pi].jumpPos2D.x;
int ptIdx = a_lineJumpFeature[pi].jumpPos2D.y;
if (scanLines[lineIdx][ptIdx].nPointIdx >= 0)
{
SWD_polarPt a_polarPt;
a_polarPt.lineIdx = lineIdx;
a_polarPt.ptIdx = ptIdx;
a_polarPt.R = 0;
a_polarPt.angle = 0;
a_polarPt.x = scanLines[lineIdx][ptIdx].pt3D.x;
a_polarPt.y = scanLines[lineIdx][ptIdx].pt3D.y;
a_polarPt.z = scanLines[lineIdx][ptIdx].pt3D.z;
polarPoints.push_back(a_polarPt);
scanLines[lineIdx][ptIdx].nPointIdx = -1;
}
}
}
for (int line = 0; line < lineNum_h_raw; line++)
{
std::vector<SSG_basicFeature1D>& a_lineJumpFeature = jumpFeatures_h_raw[line];
for (int pi = 0, pi_max = (int)a_lineJumpFeature.size(); pi < pi_max; pi++)
{
int lineIdx = a_lineJumpFeature[pi].jumpPos2D.y;
int ptIdx = a_lineJumpFeature[pi].jumpPos2D.x;
if (scanLines[lineIdx][ptIdx].nPointIdx >= 0)
{
SWD_polarPt a_polarPt;
a_polarPt.lineIdx = lineIdx;
a_polarPt.ptIdx = ptIdx;
a_polarPt.R = 0;
a_polarPt.angle = 0;
a_polarPt.x = scanLines[lineIdx][ptIdx].pt3D.x;
a_polarPt.y = scanLines[lineIdx][ptIdx].pt3D.y;
a_polarPt.z = scanLines[lineIdx][ptIdx].pt3D.z;
polarPoints.push_back(a_polarPt);
scanLines[lineIdx][ptIdx].nPointIdx = -1;
}
}
}
#else
for (int i = 0, i_max = (int)v_trees.size(); i < i_max; i++)
{
SSG_featureTree* a_vTree = &v_trees[i];
//在原始点云上标记，同时有Mask上标记
for (int j = 0, j_max = (int)a_vTree->treeNodes.size(); j < j_max; j++)
{
int lineIdx = a_vTree->treeNodes[j].jumpPos2D.x;
int ptIdx = a_vTree->treeNodes[j].jumpPos2D.y;
if (scanLines[lineIdx][ptIdx].nPointIdx >= 0)
{
SWD_polarPt a_polarPt;
a_polarPt.lineIdx = lineIdx;
a_polarPt.ptIdx = ptIdx;
a_polarPt.R = 0;
a_polarPt.angle = 0;
a_polarPt.x = scanLines[lineIdx][ptIdx].pt3D.x;
a_polarPt.y = scanLines[lineIdx][ptIdx].pt3D.y;
a_polarPt.z = scanLines[lineIdx][ptIdx].pt3D.z;
polarPoints.push_back(a_polarPt);
scanLines[lineIdx][ptIdx].nPointIdx = -1;
}
}
}
for (int i = 0, i_max = (int)h_trees.size(); i < i_max; i++)
{
SSG_featureTree* a_hTree = &h_trees[i];
//在原始点云上标记，同时有Mask上标记
for (int j = 0, j_max = (int)a_hTree->treeNodes.size(); j < j_max; j++)
{
int lineIdx = a_hTree->treeNodes[j].jumpPos2D.y;
int ptIdx = a_hTree->treeNodes[j].jumpPos2D.x;
if (scanLines[lineIdx][ptIdx].nPointIdx >= 0)
{
SWD_polarPt a_polarPt;
a_polarPt.lineIdx = lineIdx;
a_polarPt.ptIdx = ptIdx;
a_polarPt.R = 0;
a_polarPt.angle = 0;
a_polarPt.x = scanLines[lineIdx][ptIdx].pt3D.x;
a_polarPt.y = scanLines[lineIdx][ptIdx].pt3D.y;
a_polarPt.z = scanLines[lineIdx][ptIdx].pt3D.z;
polarPoints.push_back(a_polarPt);
scanLines[lineIdx][ptIdx].nPointIdx = -1;
}
}
}
#endif
//计算几何中心
int contourPtSize = (int)polarPoints.size();
if (contourPtSize == 0)
{
*errCode = SX_ERR_ZERO_CONTOUR_PT;
return workpieceCorners;
}
double center_x = 0;
double center_y = 0;
for (int pi = 0; pi < contourPtSize; pi++)
{
center_x += polarPoints[pi].x;
center_y += polarPoints[pi].y;
}
center_x = center_x / (double)contourPtSize;
center_y = center_y / (double)contourPtSize;
//计算极坐标的R和Theta
for (int pi = 0; pi < contourPtSize; pi++)
{
double angle = atan2(polarPoints[pi].y - center_y, polarPoints[pi].x - center_x);
angle = (angle / PI) * 180 +180.0;
double R = sqrt(pow(polarPoints[pi].y - center_y, 2) + pow(polarPoints[pi].x - center_x, 2));
polarPoints[pi].R = R;
polarPoints[pi].angle = angle;
}
//按角度大小排序
std::sort(polarPoints.begin(), polarPoints.end(), compareByAngle);
//提取R极值点
std::vector<int> rPeaks;
std::vector<SWD_polarPt> polarRPeakPts;
int winSize = contourPtSize / 36; //+-10度范围
if (winSize < 5)
winSize = 5;
for (int pi = 0; pi < contourPtSize; pi++)
{
double currR = polarPoints[pi].R;
bool isPeak = true;
for (int k = -winSize; k <= winSize; k++)
{
int idx = (pi + k + contourPtSize) % contourPtSize; //筒形结构
if (polarPoints[idx].R > currR)
{
isPeak = false;
break;
}
}
if (true == isPeak)
{
rPeaks.push_back(pi);
polarRPeakPts.push_back(polarPoints[pi]);
}
}
if (rPeaks.size() != 8)
{
*errCode = SX_ERR_INVLID_RPEAK_NUM;
return workpieceCorners;
}
//SSX_featureContour region[4];
//Left, top, right, bottom
SSG_intPair peakPair[4];
int pairIdx = 0;
for (int i = 0; i < 8; i++)
{
if (polarRPeakPts[i].lineIdx < 0)
continue;
//和前面的目标检测
int pre_idx = (i - 1 + 8) % 8;
int nxt_idx = (i + 1 + 8) % 8;
bool pre_isPair = false;
if (rPeaks[pre_idx] >= 0)
{
int mid_idx;
if (rPeaks[i] < rPeaks[pre_idx])
mid_idx = ((contourPtSize + rPeaks[i] + rPeaks[pre_idx]) / 2) % contourPtSize;
else
mid_idx = (rPeaks[i] + rPeaks[pre_idx]) / 2;
double mid_R = polarPoints[mid_idx].R;
cv::Point2f midChord = cv::Point2f((polarRPeakPts[i].x + polarRPeakPts[pre_idx].x) / 2, (polarRPeakPts[i].y + polarRPeakPts[pre_idx].y) / 2);
double meanR = sqrt(pow(midChord.x - center_x,2) + pow(midChord.y-center_y,2));
double delta_R = abs(mid_R - meanR);
if (delta_R < 10)
pre_isPair = true;
}
bool nxt_isPair = false;
if (rPeaks[nxt_idx] >= 0)
{
int mid_idx;
if (rPeaks[nxt_idx] < rPeaks[i])
mid_idx = ((contourPtSize + rPeaks[nxt_idx] + rPeaks[i]) / 2) % contourPtSize;
else
mid_idx = (rPeaks[i] + rPeaks[nxt_idx]) / 2;
double mid_R = polarPoints[mid_idx].R;
cv::Point2f midChord = cv::Point2f((polarRPeakPts[i].x + polarRPeakPts[nxt_idx].x) / 2, (polarRPeakPts[i].y + polarRPeakPts[nxt_idx].y) / 2);
double meanR = sqrt(pow(midChord.x - center_x, 2) + pow(midChord.y - center_y, 2));
double delta_R = abs(mid_R - meanR);
if (delta_R < 10)
nxt_isPair = true;
}
if ((true == pre_isPair) && (false == nxt_isPair))
{
peakPair[pairIdx].idx = pairIdx;
peakPair[pairIdx].data_0 = pre_idx;
peakPair[pairIdx].data_1 = i;
pairIdx++;
polarRPeakPts[pre_idx].lineIdx = -1;
polarRPeakPts[i].lineIdx = -1;
}
else if ((false == pre_isPair) && (true == nxt_isPair))
{
peakPair[pairIdx].idx = pairIdx;
peakPair[pairIdx].data_0 = i;
peakPair[pairIdx].data_1 = nxt_idx;
pairIdx++;
polarRPeakPts[nxt_idx].lineIdx = -1;
polarRPeakPts[i].lineIdx = -1;
}
else
{
*errCode = SX_ERR_INVLID_RPEAK_PAIR;
return workpieceCorners;
}
}
//生成SSX_featureContour region[4]
double centerAngle;
int idx0 = peakPair[0].data_0;
int idx1 = peakPair[0].data_1;
if (polarRPeakPts[idx0].angle > polarRPeakPts[idx1].angle)
{
centerAngle = (360 + polarRPeakPts[idx0].angle + polarRPeakPts[idx1].angle) / 2;
if (centerAngle >= 360)
centerAngle = centerAngle - 360;
}
else
centerAngle = (polarRPeakPts[idx0].angle + polarRPeakPts[idx1].angle) / 2;
int LRTB[4];
if ((centerAngle > 315) || (centerAngle <= 45)) //Left
{
LRTB[0] = 0; //left
LRTB[1] = 1; //top
LRTB[2] = 2; //right
LRTB[3] = 3; //bottom
}
else if ((centerAngle > 45) && (centerAngle <= 135)) //Top
{
LRTB[0] = 1; //top
LRTB[1] = 2; //right
LRTB[2] = 3; //bottom
LRTB[3] = 0; //left
}
else if ((centerAngle > 135) && (centerAngle <= 225)) //Right
{
LRTB[0] = 2; //right
LRTB[1] = 3; //bottom
LRTB[2] = 0; //left
LRTB[3] = 1; //top
}
else //Bottom
{
LRTB[0] = 3; //bottom
LRTB[1] = 0; //left
LRTB[2] = 1; //top
LRTB[3] = 2; //right
}
for (int i = 0; i < 4; i++)
{
int rgnIdx = LRTB[i];
int idx_0 = peakPair[i].data_0;
int idx_1 = peakPair[i].data_1;
std::vector<SVzNL3DPoint> edge;
std::vector<SVzNL3DPoint> edge_link1;
std::vector<SVzNL3DPoint> edge_link2;
int startIdx = rPeaks[idx_0];
int endIdx = rPeaks[idx_1];
if (startIdx < endIdx)
{
for (int m = startIdx + 1; m < endIdx; m++)
{
SVzNL3DPoint a_pt = { polarPoints[m].x, polarPoints[m].y, polarPoints[m].z };
edge.push_back(a_pt);
}
}
else //分两段
{
for (int m = startIdx + 1; m < contourPtSize; m++)
{
SVzNL3DPoint a_pt = { polarPoints[m].x, polarPoints[m].y, polarPoints[m].z };
edge.push_back(a_pt);
}
for (int m = 0; m < endIdx; m++)
{
SVzNL3DPoint a_pt = { polarPoints[m].x, polarPoints[m].y, polarPoints[m].z };
edge.push_back(a_pt);
}
}
//edge_link1
for (int m = 1; m < contourPtSize; m++)
{
int idx = (startIdx - m + contourPtSize) % contourPtSize;
SVzNL3DPoint a_pt = { polarPoints[idx].x, polarPoints[idx].y, polarPoints[idx].z };
double dist = sqrt(pow(a_pt.x - polarPoints[startIdx].x, 2) + pow(a_pt.y - polarPoints[startIdx].y, 2));
if (dist > workpieceParam.lineLen)
break;
edge_link1.push_back(a_pt);
}
//edge_link2
for (int m = 1; m < contourPtSize; m++)
{
int idx = (endIdx + m) % contourPtSize;
SVzNL3DPoint a_pt = { polarPoints[idx].x, polarPoints[idx].y, polarPoints[idx].z };
double dist = sqrt(pow(a_pt.x - polarPoints[endIdx].x, 2) + pow(a_pt.y - polarPoints[endIdx].y, 2));
if (dist > workpieceParam.lineLen)
break;
edge_link2.push_back(a_pt);
}
region[rgnIdx].rgnIdx = rgnIdx;
region[rgnIdx].edge.insert(region[rgnIdx].edge.end(), edge.begin(), edge.end());
region[rgnIdx].edgeLink_1.insert(region[rgnIdx].edgeLink_1.end(), edge_link1.begin(), edge_link1.end());
region[rgnIdx].edgeLink_2.insert(region[rgnIdx].edgeLink_2.end(), edge_link2.begin(), edge_link2.end());
}
}
#if 0 //老算法，需要扫描为水平-竖直方向
{
//垂直跳变特征提取 //垂直跳变特征提取
std::vector<std::vector<SSG_basicFeature1D>> jumpFeatures_v; std::vector<std::vector<SSG_basicFeature1D>> jumpFeatures_v;
for (int line = 0; line < lineNum; line++) for (int line = 0; line < lineNum; line++)
@ -403,7 +864,6 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
hTree_R = i; hTree_R = i;
} }
SSX_featureContour region[4];
region[0].rgnIdx = 0; //Left region[0].rgnIdx = 0; //Left
_getEdgeContour(&h_trees[hTree_L], region[0].edge, scanLines, false); _getEdgeContour(&h_trees[hTree_L], region[0].edge, scanLines, false);
//寻找对应的两边 //寻找对应的两边
@ -472,6 +932,11 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
return workpieceCorners; return workpieceCorners;
} }
_getEdgeLinkingContour(&v_trees[idx1], true, lastPt, region[2].edgeLink_2, scanLines, false, workpieceParam.lineLen); _getEdgeLinkingContour(&v_trees[idx1], true, lastPt, region[2].edgeLink_2, scanLines, false, workpieceParam.lineLen);
if ((region[2].edgeLink_1.size() < 5) || (region[2].edgeLink_2.size() < 5))
{
*errCode = SX_ERR_INVLD_EDGE_LINK_NUM;
return workpieceCorners;
}
region[3].rgnIdx = 3; //Bottom region[3].rgnIdx = 3; //Bottom
_getEdgeContour(&v_trees[vTree_B], region[3].edge, scanLines, true); _getEdgeContour(&v_trees[vTree_B], region[3].edge, scanLines, true);
//寻找对应的两边 //寻找对应的两边
@ -496,6 +961,8 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
*errCode = SX_ERR_INVLD_EDGE_LINK_NUM; *errCode = SX_ERR_INVLD_EDGE_LINK_NUM;
return workpieceCorners; return workpieceCorners;
} }
}
#endif
for (int i = 0; i < 4; i++) for (int i = 0; i < 4; i++)
{ {
@ -642,6 +1109,7 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
#if 1 #if 1
//将数据重新投射回原来的坐标系，以保持手眼标定结果正确 //将数据重新投射回原来的坐标系，以保持手眼标定结果正确
for (int i = 0; i < lineNum; i++) for (int i = 0; i < lineNum; i++)

3
sourceCode/BQ_workpieceCornerExtraction_Export.h
View File

@ -47,6 +47,9 @@ typedef struct
SVzNL3DPoint edge_link2_ends[2]; SVzNL3DPoint edge_link2_ends[2];
}SSX_debugInfo; }SSX_debugInfo;
//读版本号
SG_WORKPIECESHARED_EXPORT const char* wd_BQWorkpieceCornerVersion(void);
//计算一个平面调平参数。 //计算一个平面调平参数。
//数据输入中可以有一个地平面和参考调平平面，以最高的平面进行调平 //数据输入中可以有一个地平面和参考调平平面，以最高的平面进行调平
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数 //旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数

50
sourceCode/SG_baseAlgo_Export.h
View File

@ -31,7 +31,10 @@ SG_APISHARED_EXPORT void sg_lineDataRemoveOutlier_changeOriginData(
SVzNL3DPosition* lineData, SVzNL3DPosition* lineData,
int dataSize, int dataSize,
SSG_outlierFilterParam filterParam); SSG_outlierFilterParam filterParam);
//滤除离群点：z跳变门限方法, vecotr对象
SG_APISHARED_EXPORT void wd_vectorDataRemoveOutlier_overwrite(
std::vector<SVzNL3DPosition>& a_line,
SSG_outlierFilterParam filterParam);
//滤除离群点：点距离门限方法（大于门限视为不连续，根据连续段点数量判断噪声） //滤除离群点：点距离门限方法（大于门限视为不连续，根据连续段点数量判断噪声）
SG_APISHARED_EXPORT void sg_lineDataRemoveOutlier_ptDistMethod( SG_APISHARED_EXPORT void sg_lineDataRemoveOutlier_ptDistMethod(
SVzNL3DPosition* lineData, SVzNL3DPosition* lineData,
@ -39,6 +42,7 @@ SG_APISHARED_EXPORT void sg_lineDataRemoveOutlier_ptDistMethod(
SSG_outlierFilterParam filterParam, SSG_outlierFilterParam filterParam,
std::vector<SVzNL3DPosition>& filerData, std::vector<SVzNL3DPosition>& filerData,
std::vector<int>& noisePts); std::vector<int>& noisePts);
//平滑 //平滑
SG_APISHARED_EXPORT void sg_lineDataSmoothing( SG_APISHARED_EXPORT void sg_lineDataSmoothing(
std::vector<SVzNL3DPosition>& input, std::vector<SVzNL3DPosition>& input,
@ -77,6 +81,24 @@ SG_APISHARED_EXPORT void sg_getLineCornerFeature_BQ(
const SSG_cornerParam cornerPara, const SSG_cornerParam cornerPara,
std::vector<SSG_basicFeature1D>& line_features); std::vector<SSG_basicFeature1D>& line_features);
/// <summary>
/// 提取激光线上的特征：跳变、低于z阈值、V及L型，用于粒径检测（PSM)
/// seg端点：z距离大于门限
/// nPointIdx被重新定义成Feature类型
/// 算法流程：
/// （1）逐点计算前向角和后向角
/// （2）逐点计算拐角，顺时针为负，逆时针为正
/// （3）搜索正拐角的极大值。
/// （4）判断拐角是否为跳变
/// </summary>
SG_APISHARED_EXPORT void wd_getLineCornerFeature_PSM(
SVzNL3DPosition* lineData,
int dataSize,
int lineIdx,
const double groundZ,
const SSG_cornerParam cornerPara,
SSG_lineFeature* line_features);
/// <summary> /// <summary>
/// 提取激光线上的Jumping特征 /// 提取激光线上的Jumping特征
/// nPointIdx被重新定义成Feature类型 /// nPointIdx被重新定义成Feature类型
@ -142,6 +164,13 @@ SG_APISHARED_EXPORT void sg_getFeatureGrowingTrees(
std::vector<SSG_featureTree>& trees, std::vector<SSG_featureTree>& trees,
SSG_treeGrowParam growParam); SSG_treeGrowParam growParam);
//对feature进行生长:不比较type，只判断位置是否相邻
SG_APISHARED_EXPORT void wd_getFeatureGrowingTrees_noTypeMatch(
std::vector<SSG_lineFeature>& lineFeatures,
std::vector<SSG_featureTree>& feature_trees,
std::vector<SSG_featureTree>& ending_trees,
SSG_treeGrowParam growParam);
SG_APISHARED_EXPORT void sg_lineFeaturesGrowing( SG_APISHARED_EXPORT void sg_lineFeaturesGrowing(
int lineIdx, int lineIdx,
bool isLastLine, bool isLastLine,
@ -290,6 +319,10 @@ SG_APISHARED_EXPORT SVzNL3DRangeD sg_getScanDataROI_vector(
std::vector< std::vector<SVzNL3DPosition>>& scanLines std::vector< std::vector<SVzNL3DPosition>>& scanLines
); );
//计算点云的ROI和scale: vecotr格式
SG_APISHARED_EXPORT SWD_pointCloudPara wd_getPointCloudPara(
std::vector< std::vector<SVzNL3DPosition>>& scanLines);
//XY平面直线拟合 //XY平面直线拟合
SG_APISHARED_EXPORT void lineFitting( SG_APISHARED_EXPORT void lineFitting(
std::vector< SVzNL3DPoint>& inliers, std::vector< SVzNL3DPoint>& inliers,
@ -375,3 +408,18 @@ SG_APISHARED_EXPORT void sg_pointClustering(
double clusterDist, double clusterDist,
std::vector<std::vector< SVzNL3DPosition>>& objClusters //result std::vector<std::vector< SVzNL3DPosition>>& objClusters //result
); );
//对栅格化数据进行XY平面上的投影量化，并对量化产生的空白点进行插值
SG_APISHARED_EXPORT void pointClout2DProjection(
std::vector< std::vector<SVzNL3DPosition>>& gridScanData,
SVzNLRangeD x_range,
SVzNLRangeD y_range,
double scale,
int edgeSkip,
double inerPolateDistTh, //插值阈值。大于此阈值的不进行量化插值
cv::Mat& projectionData,//投影量化数据，初始化为一个极大值1e+6
cv::Mat& backIndexing //标记坐标索引，用于回找3D坐标
);
//分水岭算法
SG_APISHARED_EXPORT void watershed(SWD_waterShedImage& img);

35
sourceCode/SG_baseDataType.h
View File

@ -4,7 +4,7 @@
#include <vector> #include <vector>
#include <SG_errCode.h> #include <SG_errCode.h>
#define PI 3.141592654 #define PI 3.14159265358979323846
// 定义欧拉角结构体（单位：度） // 定义欧拉角结构体（单位：度）
typedef struct{ typedef struct{
@ -199,6 +199,22 @@ typedef struct
double bagH; //高 double bagH; //高
}SSG_bagParam; }SSG_bagParam;
typedef struct
{
double length; //长
double width; //宽
double height; //高
}SWD_sizeParam;
typedef struct
{
SVzNLRangeD xRange; //< X范围
SVzNLRangeD yRange; //< Y范围
SVzNLRangeD zRange; //< Z范围
double scale_x;
double scale_y;
} SWD_pointCloudPara;
typedef struct typedef struct
{ {
ESG_poseSortingMode sortMode; ESG_poseSortingMode sortMode;
@ -267,6 +283,15 @@ typedef struct
int value; int value;
}SSG_RUN; }SSG_RUN;
typedef struct
{
int start;
int len;
int value;
bool start_zRising; //起点z上跳标志
bool end_zRising;//终点z上跳标志
}SSG_RUN_EX;
typedef struct typedef struct
{ {
double mean; double mean;
@ -414,3 +439,11 @@ typedef struct
double forward_z; double forward_z;
double backward_z; double backward_z;
}SSG_pntDirAngle; }SSG_pntDirAngle;
// 图像数据结构
typedef struct {
int width;
int height;
std::vector<std::vector<int>> gray; // 灰度图
std::vector<std::vector<int>> markers; // 标记图（-1表示分水岭，0表示未标记，>0表示区域）
}SWD_waterShedImage;

183
sourceCode/SG_baseFunc.cpp
View File

@ -7,6 +7,7 @@
#include <cmath> #include <cmath>
#include <unordered_map> #include <unordered_map>
//计算扫描ROI
SVzNL3DRangeD sg_getScanDataROI( SVzNL3DRangeD sg_getScanDataROI(
SVzNL3DLaserLine* laser3DPoints, SVzNL3DLaserLine* laser3DPoints,
int lineNum) int lineNum)
@ -67,6 +68,7 @@ SVzNL3DRangeD sg_getScanDataROI(
return roi; return roi;
} }
//计算扫描ROI: vecotr格式
SVzNL3DRangeD sg_getScanDataROI_vector(std::vector< std::vector<SVzNL3DPosition>>& scanLines) SVzNL3DRangeD sg_getScanDataROI_vector(std::vector< std::vector<SVzNL3DPosition>>& scanLines)
{ {
SVzNL3DRangeD roi; SVzNL3DRangeD roi;
@ -127,6 +129,93 @@ SVzNL3DRangeD sg_getScanDataROI_vector(std::vector< std::vector<SVzNL3DPosition>
return roi; return roi;
} }
//计算点云的ROI和scale: vecotr格式
SWD_pointCloudPara wd_getPointCloudPara(std::vector< std::vector<SVzNL3DPosition>>& scanLines)
{
SWD_pointCloudPara para;
para.xRange = { 0, -1 };
para.yRange = { 0, -1 };
para.zRange = { 0, -1 };
para.scale_x = -1; //初始值
para.scale_y = -1;
int lineNum = (int)scanLines.size();
double x_scale = 0;
int x_scale_cnt = 0;
double y_scale = 0;
double y_scale_cnt = 0;
for (int line = 0; line < lineNum; line++)
{
int nPositionCnt = (int)scanLines[line].size();
for (int i = 0; i < nPositionCnt; i++)
{
SVzNL3DPosition* pt3D = &scanLines[line][i];
if (pt3D->pt3D.z < 1e-4)
continue;
if (i > 0)
{
if (scanLines[line][i - 1].pt3D.z > 1e-4)
{
y_scale += abs(pt3D->pt3D.y - scanLines[line][i - 1].pt3D.y);
y_scale_cnt++;
}
}
if (line > 0)
{
if (scanLines[line - 1][i].pt3D.z > 1e-4)
{
x_scale += abs(pt3D->pt3D.x - scanLines[line-1][i].pt3D.x);
x_scale_cnt++;
}
}
if (para.xRange.max < para.xRange.min)
{
para.xRange.min = pt3D->pt3D.x;
para.xRange.max = pt3D->pt3D.x;
}
else
{
if (para.xRange.min > pt3D->pt3D.x)
para.xRange.min = pt3D->pt3D.x;
if (para.xRange.max < pt3D->pt3D.x)
para.xRange.max = pt3D->pt3D.x;
}
//y
if (para.yRange.max < para.yRange.min)
{
para.yRange.min = pt3D->pt3D.y;
para.yRange.max = pt3D->pt3D.y;
}
else
{
if (para.yRange.min > pt3D->pt3D.y)
para.yRange.min = pt3D->pt3D.y;
if (para.yRange.max < pt3D->pt3D.y)
para.yRange.max = pt3D->pt3D.y;
}
//z
if (para.zRange.max < para.zRange.min)
{
para.zRange.min = pt3D->pt3D.z;
para.zRange.max = pt3D->pt3D.z;
}
else
{
if (para.zRange.min > pt3D->pt3D.z)
para.zRange.min = pt3D->pt3D.z;
if (para.zRange.max < pt3D->pt3D.z)
para.zRange.max = pt3D->pt3D.z;
}
}
}
if (x_scale_cnt > 0)
para.scale_x = x_scale / (double)x_scale_cnt;
if (y_scale_cnt > 0)
para.scale_y = y_scale / (double)y_scale_cnt;
return para;
}
void lineFitting(std::vector< SVzNL3DPoint>& inliers, double* _k, double* _b) void lineFitting(std::vector< SVzNL3DPoint>& inliers, double* _k, double* _b)
{ {
//最小二乘拟合直线参数 //最小二乘拟合直线参数
@ -1982,3 +2071,97 @@ void lineDataRT_RGBD(SVzNLXYZRGBDLaserLine* a_line, const double* camPoseR, doub
} }
return; return;
} }
//对栅格化数据进行XY平面上的投影量化，并对量化产生的空白点进行插值
void pointClout2DProjection(
std::vector< std::vector<SVzNL3DPosition>>& gridScanData,
SVzNLRangeD x_range,
SVzNLRangeD y_range,
double scale,
int edgeSkip,
double inerPolateDistTh, //插值阈值。大于此阈值的不进行量化插值
cv::Mat& projectionData,//投影量化数据，初始化为一个极大值1e+6
cv::Mat& backIndexing //标记坐标索引，用于回找3D坐标
)
{
int lineNum = (int)gridScanData.size();
if (lineNum == 0)
return;
int nPointCnt = (int)gridScanData[0].size();
for (int line = 0; line < lineNum; line++)
{
int pre_x = -1, pre_y = -1;
SVzNL3DPosition* prePt = NULL;
for (int i = 0; i < nPointCnt; i++)
{
SVzNL3DPosition* pt3D = &gridScanData[line][i];
if (pt3D->pt3D.z < 1e-4)
continue;
double x = pt3D->pt3D.x;
double y = pt3D->pt3D.y;
int px = (int)(x - x_range.min)/scale + edgeSkip;
int py = (int)(y - y_range.min)/scale + edgeSkip;
cv::Vec2i v2i_exist = backIndexing.at<cv::Vec2i>(py, px);
#if 0
if ((v2i_exist[0] > 0) || (v2i_exist[1] > 0)) //多个点重复投影到同一个点上，只保留一个有效点
{
pt3D->pt3D.z = 0; //invalidate
}
else
#endif
{
cv::Vec2i v2i = { line, i };
backIndexing.at<cv::Vec2i>(py, px) = v2i;
projectionData.at<float>(py, px) = 1e+6;
//垂直插值
if (prePt)
{
//计算距离，超过一定距离则不插值
double dist = sqrt(pow(pt3D->pt3D.x - prePt->pt3D.x, 2) +
pow(pt3D->pt3D.y - prePt->pt3D.y, 2) +
pow(pt3D->pt3D.z - prePt->pt3D.z, 2));
if (dist < inerPolateDistTh)
{
std::vector<SVzNL2DPoint> interPts;
drawLine(
pre_x,
pre_y,
px,
py,
interPts);
for (int m = 0, m_max = (int)interPts.size(); m < m_max; m++)
projectionData.at<float>(interPts[m].y, interPts[m].x) = 1e+6;
}
}
prePt = pt3D;
pre_x = px;
pre_y = py;
}
}
}
//水平插值
int pixWin = (int)(inerPolateDistTh / scale);
for (int y = 0; y < projectionData.rows; y++)
{
int pre_x = -1;
for (int x = 0; x < projectionData.cols; x++)
{
double value = projectionData.at<float>(y, x);
if (value > 1e-4)
{
if (pre_x >= 0)
{
//插值
int x_diff = x - pre_x;
if ((x_diff > 1) && (x_diff < pixWin))
{
for (int m = pre_x + 1; m < x; m++)
projectionData.at<float>(y, m) = 1e+6;
}
}
pre_x = x;
}
}
}
}

3
sourceCode/SG_errCode.h
View File

@ -12,3 +12,6 @@
#define SX_ERR_INVLD_HTREE_NUM -2002 #define SX_ERR_INVLD_HTREE_NUM -2002
#define SX_ERR_INVLD_EDGE_LINK_NUM -2003 #define SX_ERR_INVLD_EDGE_LINK_NUM -2003
#define SX_ERR_INVLD_CLOSES_PT -2004 #define SX_ERR_INVLD_CLOSES_PT -2004
#define SX_ERR_ZERO_CONTOUR_PT -2005
#define SX_ERR_INVLID_RPEAK_NUM -2006
#define SX_ERR_INVLID_RPEAK_PAIR -2007

121
sourceCode/SG_featureGrow.cpp
View File

@ -82,6 +82,37 @@ bool _featureGrowing(SSG_basicFeature1D& a_feature, const int lineIdx, std::vect
return false; return false;
} }
//将feature在trees上寻找合适的生长点进行生长。如果没有合适的生长点， 返回false
//没有使用全匹配。一个feature一旦被匹配上，匹配就完成。没有使用最佳匹配。
bool _featureGrowing_noTypeMatch(SSG_basicFeature1D& a_feature, const int lineIdx, std::vector<SSG_featureTree>& trees, SSG_treeGrowParam growParam)
{
for (int i = 0, i_max = (int)trees.size(); i < i_max; i++)
{
SSG_featureTree& a_tree = trees[i];
if (TREE_STATE_DEAD == a_tree.treeState)
continue;
//检查生长点
SSG_basicFeature1D last_node = a_tree.treeNodes.back();
if (last_node.jumpPos2D.x == a_feature.jumpPos2D.x) //x为lineIdx，同一条扫描线上的不进行生长
continue;
//判断生长点
double y_diff = abs(a_feature.jumpPos.y - last_node.jumpPos.y);
double z_diff = abs(a_feature.jumpPos.z - last_node.jumpPos.z);
int line_diff = abs(a_feature.jumpPos2D.x - last_node.jumpPos2D.x);
double x_diff = abs(a_feature.jumpPos.x - last_node.jumpPos.x);
if ((y_diff < growParam.yDeviation_max) && (z_diff < growParam.zDeviation_max) &&
((line_diff < growParam.maxLineSkipNum) || (x_diff < growParam.maxSkipDistance)))
{
a_tree.eLineIdx = lineIdx;
a_tree.treeNodes.push_back(a_feature);
a_tree.tree_value += a_feature.featureValue;
return true;
}
}
return false;
}
int _getMatchedTree_angleCheck( int _getMatchedTree_angleCheck(
SSG_basicFeature1D& a_feature, SSG_basicFeature1D& a_feature,
const int lineIdx, const int lineIdx,
@ -481,6 +512,95 @@ void sg_getFeatureGrowingTrees(
} }
} }
void wd_getFeatureGrowingTrees_noTypeMatch(
std::vector<SSG_lineFeature>& lineFeatures,
std::vector<SSG_featureTree>& feature_trees,
std::vector<SSG_featureTree>& ending_trees,
SSG_treeGrowParam growParam)
{
for (int i = 0, i_max = (int)lineFeatures.size(); i < i_max; i++)
{
SSG_lineFeature& a_line = lineFeatures[i];
if (a_line.features.size() > 0)
{
for (int j = 0, j_max = (int)a_line.features.size(); j < j_max; j++)
{
SSG_basicFeature1D& a_feature = a_line.features[j];
if (a_feature.jumpPos2D.x == 207)
int kkk = 1;
bool isMatched = _featureGrowing_noTypeMatch(a_feature, a_line.lineIdx, feature_trees, growParam);
if (false == isMatched)
{
//新的生长树
SSG_featureTree a_newTree;
a_newTree.treeNodes.push_back(a_feature);
a_newTree.treeState = TREE_STATE_ALIVE;
a_newTree.treeType = a_feature.featureType;
a_newTree.sLineIdx = a_line.lineIdx;
a_newTree.eLineIdx = a_line.lineIdx;
feature_trees.push_back(a_newTree);
}
}
}
if (a_line.endings.size() > 0)
{
for (int j = 0, j_max = (int)a_line.endings.size(); j < j_max; j++)
{
SSG_basicFeature1D& a_feature = a_line.endings[j];
if (a_feature.jumpPos2D.x == 207)
int kkk = 1;
bool isMatched = _featureGrowing_noTypeMatch(a_feature, a_line.lineIdx, ending_trees, growParam);
if (false == isMatched)
{
//新的生长树
SSG_featureTree a_newTree;
a_newTree.treeNodes.push_back(a_feature);
a_newTree.treeState = TREE_STATE_ALIVE;
a_newTree.treeType = a_feature.featureType;
a_newTree.sLineIdx = a_line.lineIdx;
a_newTree.eLineIdx = a_line.lineIdx;
ending_trees.push_back(a_newTree);
}
}
}
//检查停止生长的树,加速。
//将生长节点为1的生长树移除
int lineIdx = a_line.lineIdx;
int m_max = (int)feature_trees.size();
for (int m = m_max - 1; m >= 0; m--) //从后往前，这样删除不会影响循环
{
if (TREE_STATE_ALIVE == feature_trees[m].treeState)
{
int line_diff = abs(lineIdx - feature_trees[m].treeNodes.back().jumpPos2D.x);
if (((growParam.maxLineSkipNum > 0) && (line_diff > growParam.maxLineSkipNum)) ||
(i == i_max - 1))
{
feature_trees[m].treeState = TREE_STATE_DEAD;
bool isValid = _invalidateVSlopeTrees(feature_trees[m], growParam.minLTypeTreeLen, growParam.minVTypeTreeLen);
if (false == isValid)
feature_trees.erase(feature_trees.begin() + m);
}
}
}
m_max = (int)ending_trees.size();
for (int m = m_max - 1; m >= 0; m--) //从后往前，这样删除不会影响循环
{
if (TREE_STATE_ALIVE == ending_trees[m].treeState)
{
int line_diff = abs(lineIdx - ending_trees[m].treeNodes.back().jumpPos2D.x);
if (((growParam.maxLineSkipNum > 0) && (line_diff > growParam.maxLineSkipNum)) ||
(i == i_max - 1))
{
ending_trees[m].treeState = TREE_STATE_DEAD;
bool isValid = _invalidateVSlopeTrees(ending_trees[m], growParam.minLTypeTreeLen, growParam.minVTypeTreeLen);
if (false == isValid)
ending_trees.erase(ending_trees.begin() + m);
}
}
}
}
}
void sg_lineFeaturesGrowing( void sg_lineFeaturesGrowing(
int lineIdx, int lineIdx,
bool isLastLine, bool isLastLine,
@ -811,7 +931,6 @@ if (edge1_pts[i].x == 622)
#endif #endif
} }
void sg_getPeakGrowingTrees( void sg_getPeakGrowingTrees(
std::vector<std::vector< SSG_basicFeature1D>>& peakFeatures, std::vector<std::vector< SSG_basicFeature1D>>& peakFeatures,
std::vector<SSG_featureTree>& trees, std::vector<SSG_featureTree>& trees,

400
sourceCode/SG_lineFeature.cpp
View File

@ -261,6 +261,62 @@ void sg_lineDataRemoveOutlier_changeOriginData(
return; return;
} }
//滤除离群点：z跳变门限方法, vecotr对象
void wd_vectorDataRemoveOutlier_overwrite(
std::vector<SVzNL3DPosition>& a_line,
SSG_outlierFilterParam filterParam)
{
int dataSize = (int)a_line.size();
std::vector< SSG_RUN> continueRuns;
SSG_RUN a_run = { 0, -1, 0 }; //startIdx, len, lastIdx
double pre_z = 0;
for (int i = 0; i < dataSize; i++)
{
if (i == 370)
int kkk = 1;
if (a_line[i].pt3D.z > 1e-4)
{
if (a_run.len < 0)
{
a_run.start = i;
a_run.len = 1;
a_run.value = i;
}
else
{
double z_diff = abs(a_line[i].pt3D.z - pre_z);
if (z_diff < filterParam.continuityTh)
{
a_run.len++;
a_run.value = i;
}
else
{
continueRuns.push_back(a_run);
a_run.start = i;
a_run.len = 1;
a_run.value = i;
}
}
pre_z = a_line[i].pt3D.z;
}
}
if (a_run.len > 0)
continueRuns.push_back(a_run);
for (int i = 0, i_max = (int)continueRuns.size(); i < i_max; i++)
{
if (continueRuns[i].len < filterParam.outlierTh) //噪声
{
for (int j = continueRuns[i].start; j <= continueRuns[i].value; j++)
{
a_line[j].pt3D.z = 0;
}
}
}
return;
}
//滤除离群点：点距离方法 //滤除离群点：点距离方法
void sg_lineDataRemoveOutlier_ptDistMethod(SVzNL3DPosition* lineData, int dataSize, SSG_outlierFilterParam filterParam, std::vector<SVzNL3DPosition>& filerData, std::vector<int>& noisePts) void sg_lineDataRemoveOutlier_ptDistMethod(SVzNL3DPosition* lineData, int dataSize, SSG_outlierFilterParam filterParam, std::vector<SVzNL3DPosition>& filerData, std::vector<int>& noisePts)
{ {
@ -1134,6 +1190,7 @@ void sg_getLineLVFeature(SVzNL3DPosition* lineData, int dataSize, int lineIdx, c
/// <summary> /// <summary>
/// 提取激光线上的拐点特征 /// 提取激光线上的拐点特征
/// seg端点：z距离大于门限
/// nPointIdx被重新定义成Feature类型 /// nPointIdx被重新定义成Feature类型
/// 算法流程： /// 算法流程：
/// （1）逐点计算前向角和后向角 /// （1）逐点计算前向角和后向角
@ -1445,6 +1502,7 @@ void sg_getLineCornerFeature(
/// <summary> /// <summary>
/// 提取激光线上的拐点特征，水平端点视为Corner /// 提取激光线上的拐点特征，水平端点视为Corner
/// seg端点：z距离或y距离大于门限
/// 根据Seg进行corner提取 /// 根据Seg进行corner提取
/// nPointIdx被重新定义成Feature类型 /// nPointIdx被重新定义成Feature类型
/// 算法流程： /// 算法流程：
@ -1787,11 +1845,351 @@ void sg_getLineCornerFeature_BQ(
return; return;
} }
/// <summary>
/// 提取激光线上的特征：跳变、低于z阈值、V及L型，用于粒径检测（PSM)
/// seg端点：z距离大于门限
/// nPointIdx被重新定义成Feature类型
/// 算法流程：
/// （1）逐点计算前向角和后向角
/// （2）逐点计算拐角，顺时针为负，逆时针为正
/// （3）搜索正拐角的极大值。
/// （4）判断拐角是否为跳变
/// </summary>
void wd_getLineCornerFeature_PSM(
SVzNL3DPosition* lineData,
int dataSize,
int lineIdx,
const double groundZ, //地平面Z，大于grondZ的点视为目标边界
const SSG_cornerParam cornerPara,
SSG_lineFeature* line_features)
{
line_features->lineIdx = lineIdx;
if (lineIdx == 538)
int kkk = 1;
//对groundZ进行截断
for (int i = 0; i < dataSize; i++)
{
if (lineData[i].pt3D.z > groundZ)
lineData[i].pt3D.z = 0;
}
//去除零点
std::vector< SVzNL3DPosition> vldPts;
std::vector< int> vldPtSegIdx;
std::vector<SSG_RUN_EX> segs;
std::vector<int> backIndexing;
backIndexing.resize(dataSize);
int runIdx = 1;
SSG_RUN_EX a_run = { 0, -1, 0, false, false }; //startIdx, len, lastIdx
double pre_z = 0;
double pre_y = 0;
for (int i = 0; i < dataSize; i++)
{
if (i == 370)
int kkk = 1;
if (lineData[i].pt3D.z > 1e-4)
{
if (a_run.len < 0)
{
a_run.start_zRising = true;
a_run.start = i;
a_run.len = 1;
a_run.end_zRising = true;
a_run.value = i;
}
else
{
double z_diff = abs(lineData[i].pt3D.z - pre_z);
if (z_diff < cornerPara.minEndingGap_z)
{
a_run.len = i - a_run.start + 1;
a_run.value = i;
}
else
{
bool next_zRising;
if (pre_z > lineData[i].pt3D.z)
{
a_run.end_zRising = true;
next_zRising = false;
}
else
{
a_run.end_zRising = false;
next_zRising = true;
}
a_run.value = runIdx;
runIdx++;
segs.push_back(a_run);
a_run.start = i;
a_run.start_zRising = next_zRising;
a_run.len = 1;
a_run.value = i;
a_run.end_zRising = true;
}
}
int bIdx = (int)vldPts.size();
backIndexing[i] = bIdx;
vldPts.push_back(lineData[i]);
vldPtSegIdx.push_back(runIdx);
pre_z = lineData[i].pt3D.z;
}
}
if (a_run.len > 0)
segs.push_back(a_run);
//将点置标志
for (int i = 0, i_max = (int)segs.size(); i < i_max; i++)
{
int idx1 = segs[i].start;
int idx2 = segs[i].start + segs[i].len - 1;
lineData[idx1].nPointIdx |= 0x100000;
lineData[idx2].nPointIdx |= 0x200000;
}
//计算前向角和后向角
std::vector< SSG_pntDirAngle> corners;
corners.resize(vldPts.size());
for (int i = 0, i_max = (int)vldPts.size(); i < i_max; i++)
{
if ((lineIdx == 399) && (i == 419))
int kkk = 1;
//前向寻找
int pre_i = -1;
for (int j = i - 1; j >= 0; j--)
{
double dist = sqrt(pow(vldPts[i].pt3D.y - vldPts[j].pt3D.y, 2) +
pow(vldPts[i].pt3D.z - vldPts[j].pt3D.z, 2));
if (dist >= cornerPara.scale)
{
pre_i = j;
break;
}
}
//后向寻找
int post_i = -1;
for (int j = i + 1; j < i_max; j++)
{
double dist = sqrt(pow(vldPts[i].pt3D.y - vldPts[j].pt3D.y, 2) +
pow(vldPts[i].pt3D.z - vldPts[j].pt3D.z, 2));
if (dist >= cornerPara.scale)
{
post_i = j;
break;
}
}
//计算拐角
if ((pre_i < 0) || (post_i < 0))
{
corners[i].pntIdx = -1;
corners[i].forwardAngle = 0;
corners[i].backwardAngle = 0;
corners[i].corner = 0;
corners[i].forwardDiffZ = 0;
corners[i].backwardDiffZ = 0;
}
else
{
double tanValue_pre = (vldPts[i].pt3D.z - vldPts[pre_i].pt3D.z) / abs(vldPts[i].pt3D.y - vldPts[pre_i].pt3D.y);
double tanValue_post = (vldPts[post_i].pt3D.z - vldPts[i].pt3D.z) / abs(vldPts[post_i].pt3D.y - vldPts[i].pt3D.y);
double forwardAngle = atan(tanValue_post) * 180.0 / PI;
double backwardAngle = atan(tanValue_pre) * 180.0 / PI;
corners[i].pntIdx = i;
corners[i].forwardAngle = forwardAngle;
corners[i].backwardAngle = backwardAngle;
corners[i].corner = -(forwardAngle - backwardAngle); //图像坐标系与正常坐标系y方向相反，所以有“-”号
corners[i].forwardDiffZ = vldPts[post_i].pt3D.z - vldPts[i].pt3D.z;
corners[i].backwardDiffZ = vldPts[i].pt3D.z - vldPts[pre_i].pt3D.z;
}
}
//搜索拐角极值
int _state = 0;
int pre_i = -1;
int sEdgePtIdx = -1;
int eEdgePtIdx = -1;
SSG_pntDirAngle* pre_data = NULL;
std::vector< SSG_pntDirAngle> cornerPeakP;
std::vector< SSG_pntDirAngle> cornerPeakM;
for (int i = 0, i_max = (int)vldPts.size(); i < i_max; i++)
{
if (i == 275)
int kkk = 1;
SSG_pntDirAngle* curr_data = &corners[i];
if (curr_data->pntIdx < 0)
{
if (i == i_max - 1) //最后一个
{
if (1 == _state) //上升
{
cornerPeakP.push_back(corners[eEdgePtIdx]);
}
else if (2 == _state) //下降
{
cornerPeakM.push_back(corners[eEdgePtIdx]);
}
}
continue;
}
if (NULL == pre_data)
{
sEdgePtIdx = i;
eEdgePtIdx = i;
pre_data = curr_data;
pre_i = i;
continue;
}
eEdgePtIdx = i;
double cornerDiff = curr_data->corner - pre_data->corner;
switch (_state)
{
case 0: //初态
if (cornerDiff < 0) //下降
{
_state = 2;
}
else if (cornerDiff > 0) //上升
{
_state = 1;
}
break;
case 1: //上升
if (cornerDiff < 0) //下降
{
cornerPeakP.push_back(*pre_data);
_state = 2;
}
break;
case 2: //下降
if (cornerDiff > 0) // 上升
{
cornerPeakM.push_back(*pre_data);
_state = 1;
}
break;
default:
_state = 0;
break;
}
pre_data = curr_data;
pre_i = i;
}
//注意：最后一个不处理，为基座位置
//极小值点（峰顶）
//极值比较，在尺度窗口下寻找局部极值点
double square_distTh = 4 * cornerPara.scale * cornerPara.scale; //2倍的cornerScale。
for (int i = 0, i_max = (int)cornerPeakP.size(); i < i_max; i++)
{
if (cornerPeakP[i].corner < cornerPara.cornerTh)
continue;
bool isPeak = true;
//向前搜索
int cornerPtIdx = cornerPeakP[i].pntIdx;
for (int j = i - 1; j >= 0; j--)
{
int prePtIdx = cornerPeakP[j].pntIdx;
double dist = pow(vldPts[cornerPtIdx].pt3D.y - vldPts[prePtIdx].pt3D.y, 2); // + pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2) ;
if (dist > square_distTh) //超出尺度窗口
break;
if (cornerPeakP[i].corner < cornerPeakP[j].corner)
{
isPeak = false;
break;
}
}
//向后搜索
if (true == isPeak)
{
cornerPtIdx = cornerPeakP[i].pntIdx;
for (int j = i + 1; j < i_max; j++)
{
int postPtIdx = cornerPeakP[j].pntIdx;
double dist = pow(vldPts[cornerPtIdx].pt3D.y - vldPts[postPtIdx].pt3D.y, 2); // +pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2);
if (dist > square_distTh) //超出尺度窗口
break;
if (cornerPeakP[i].corner < cornerPeakP[j].corner)
{
isPeak = false;
break;
}
}
}
if (true == isPeak)
{
SSG_basicFeature1D a_feature;
if ((cornerPeakP[i].backwardAngle > cornerPara.jumpCornerTh_1) && (cornerPeakP[i].forwardAngle > -cornerPara.jumpCornerTh_2))
a_feature.featureType = LINE_FEATURE_L_JUMP_H2L;
else if ((cornerPeakP[i].forwardAngle < -cornerPara.jumpCornerTh_1) && (cornerPeakP[i].backwardAngle < cornerPara.jumpCornerTh_2))
a_feature.featureType = LINE_FEATURE_L_JUMP_L2H;
else
a_feature.featureType = LINE_FEATURE_CORNER_V;
a_feature.jumpPos = vldPts[cornerPtIdx].pt3D;
a_feature.jumpPos2D = { lineIdx, vldPts[cornerPtIdx].nPointIdx };
line_features->features.push_back(a_feature);
}
}
//添加开始和结束边界
//检查seg是否需要合并；向后合并
for (int i = 0, i_max = (int)segs.size(); i < i_max - 1; i++)
{
SSG_RUN_EX* nxt_seg = &segs[i + 1];
SSG_RUN_EX* curr_seg = &segs[i];
int idx_1 = curr_seg->start + curr_seg->len - 1;
int idx_2 = nxt_seg->start;
double y_diff = abs(lineData[idx_1].pt3D.y - lineData[idx_2].pt3D.y);
double z_diff = abs(lineData[idx_1].pt3D.z - lineData[idx_2].pt3D.z);
if ((y_diff < cornerPara.minEndingGap) && (z_diff < cornerPara.minEndingGap_z)) //合并
{
int idx_end = nxt_seg->start + nxt_seg->len - 1;
nxt_seg->start = curr_seg->start;
nxt_seg->len = idx_end - curr_seg->start + 1;
curr_seg->value = 0;
}
}
for (int i = 0, i_max = (int)segs.size(); i < i_max; i++)
{
if (0 == segs[i].value) //被合并
continue;
int idx_1 = segs[i].start;
int idx_2 = segs[i].start + segs[i].len - 1;
SSG_basicFeature1D an_edge;
memset(&an_edge, 0, sizeof(SSG_basicFeature1D));
an_edge.featureType = LINE_FEATURE_LINE_ENDING_0;
an_edge.jumpPos = lineData[idx_1].pt3D;
an_edge.jumpPos2D = { lineIdx, idx_1 };
line_features->endings.push_back(an_edge);
//line_features.insert(line_features.begin(), an_edge); //头部
//尾部
an_edge.featureType = LINE_FEATURE_LINE_ENDING_1;
an_edge.jumpPos = lineData[idx_2].pt3D;
an_edge.jumpPos2D = { lineIdx, idx_2 };
line_features->endings.push_back(an_edge);
}
return;
}
bool compareByIdx(const SSG_pntDirAngle& a, const SSG_pntDirAngle& b) { bool compareByIdx(const SSG_pntDirAngle& a, const SSG_pntDirAngle& b) {
return a.pntIdx < b.pntIdx; return a.pntIdx < b.pntIdx;
} }
/// <summary> /// <summary>
/// 提取激光线上的Jumping特征 /// 提取激光线上的Jumping特征, 用于搭接焊缝
/// seg端点：z距离大于门限
/// nPointIdx被重新定义成Feature类型 /// nPointIdx被重新定义成Feature类型
/// 算法流程： /// 算法流程：
/// （1）逐点计算前向角和后向角 /// （1）逐点计算前向角和后向角

1
sourceCode/SG_regionGrow.cpp
View File

@ -1359,4 +1359,3 @@ void sg_getLocalPeaks_distTransform(cv::Mat& input, std::vector<SSG_2DValueI>& p
} }
return; return;
} }