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

BQ_workpieceCornerExtract Version:1.1.0

Browse Source
This commit is contained in:
jerryzeng 2025-11-08 02:13:19 +08:00
parent 5b658bbef7
commit db2ad9337e
14 changed files with 1815 additions and 341 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
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;
}
if (debugData)
if ((debugData) && (workpieceCorner.workpieceType > 0))
{
int linePtNum = debugData[0].edge_size + debugData[0].edgeLink1_size + debugData[0].edgeLink2_size;
linePtNum += debugData[1].edge_size + debugData[1].edgeLink1_size + debugData[1].edgeLink2_size;
@ -606,15 +606,20 @@ void _outputRGBDScanLapWeld_RGBD(
#define CONVERT_TO_GRID 0
#define TEST_COMPUTE_CALIB_PARA 0
#define TEST_COMPUTE_CORNER 1
#define TEST_GROUP 1
#define TEST_GROUP 6
int main()
{
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] = {
{1,3}
{1,3}, {6,8}, {6,8}, {6,8}, {6,8}, {9,12}
};
#if CONVERT_TO_GRID
@ -641,7 +646,7 @@ int main()
#if TEST_COMPUTE_CALIB_PARA
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;
float lineV = 0.0f;
int dataCalib = 0;
@ -677,10 +682,10 @@ int main()
}
//
char calibFile[250];
sprintf_s(calibFile, "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\ground_calib_para.txt");
sprintf_s(calibFile, "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\工件点云\\ground_calib_para.txt");
_outputCalibPara(calibFile, calibPara);
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;
_outputScanDataFile_vector(_out_file, scanData, false, &headNullLines);
printf("%s: calib done!\n", _calib_datafile);
@ -688,7 +693,10 @@ int main()
#endif
#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;
//初始化成单位阵
@ -710,12 +718,40 @@ int main()
sprintf_s(calibFile, "F:\\ShangGu\\项目\\冠钦_博清科技\\数据\\ground_calib_para.txt");
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++)
{
//fidx =1;
char _scan_file[256];
sprintf_s(_scan_file, "%sscanData_%d_grid.txt", dataPath[grp], fidx);
if(0 == grp)
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;
vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines);

170
QRcode3Ddetection_test/QRcode3Ddetection_test.cpp
View File

@ -73,6 +73,95 @@ void vzReadLaserScanPointFromFile_plyTxt(const char* fileName, std::vector< SVzN
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)
{
std::ofstream sw(fileName);
@ -133,6 +222,46 @@ void _outputScanDataFile_vector(char* fileName, std::vector<std::vector<SVzNL3DP
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(
char* fileName,
std::vector<std::vector<SVzNL3DPosition>>& scanLines,
@ -562,20 +691,53 @@ void _getRoiClouds(
return;
}
#define CONVERT_TO_GRID 1
#define TEST_COMPUTE_CALIB_PARA 0
#define TEST_COMPUTE_QRCODE_IMG 1
#define TEST_GROUP 2
#define TEST_COMPUTE_QRCODE_IMG 0
#define TEST_GROUP 2
int main()
{
const char* dataPath[TEST_GROUP] = {
"F:\\ShangGu\\项目\\工件端部圆点二维码\\", //0
"F:\\ShangGu\\项目\\工件端部圆点二维码\\字符\\" //1
"F:\\ShangGu\\项目\\国铭铸管\\二维码\\", //0
"F:\\ShangGu\\项目\\国铭铸管\\字符\\" //1
};
SVzNLRange fileIdx[TEST_GROUP] = {
{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
char _calib_datafile[256];
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}
EndProjectSection
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
GlobalSection(SolutionConfigurationPlatforms) = preSolution
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|x86.ActiveCfg = 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
GlobalSection(SolutionProperties) = preSolution
HideSolutionNode = FALSE

6
bagPositioning_test/bagPositioning_test.cpp
View File

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

1
baseAlgorithm/baseAlgorithm.vcxproj
View File

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

1097
sourceCode/BQ_workpieceCornerExtraction.cpp
View File

File diff suppressed because it is too large Load Diff

3
sourceCode/BQ_workpieceCornerExtraction_Export.h
View File

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

52
sourceCode/SG_baseAlgo_Export.h
View File

@ -22,7 +22,10 @@ SG_APISHARED_EXPORT void sg_lineDataRemoveOutlier_changeOriginData(
SVzNL3DPosition* lineData,
int dataSize,
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(
SVzNL3DPosition* lineData,
@ -30,6 +33,7 @@ SG_APISHARED_EXPORT void sg_lineDataRemoveOutlier_ptDistMethod(
SSG_outlierFilterParam filterParam,
std::vector<SVzNL3DPosition>& filerData,
std::vector<int>& noisePts);
//平滑
SG_APISHARED_EXPORT void sg_lineDataSmoothing(
std::vector<SVzNL3DPosition>& input,
@ -68,6 +72,24 @@ SG_APISHARED_EXPORT void sg_getLineCornerFeature_BQ(
const SSG_cornerParam cornerPara,
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>
/// 提取激光线上的Jumping特征
/// nPointIdx被重新定义成Feature类型
@ -133,6 +155,13 @@ SG_APISHARED_EXPORT void sg_getFeatureGrowingTrees(
std::vector<SSG_featureTree>& trees,
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(
int lineIdx,
bool isLastLine,
@ -281,6 +310,10 @@ SG_APISHARED_EXPORT SVzNL3DRangeD sg_getScanDataROI_vector(
std::vector< std::vector<SVzNL3DPosition>>& scanLines
);
//计算点云的ROI和scale: vecotr格式
SG_APISHARED_EXPORT SWD_pointCloudPara wd_getPointCloudPara(
std::vector< std::vector<SVzNL3DPosition>>& scanLines);
//XY平面直线拟合
SG_APISHARED_EXPORT void lineFitting(
std::vector< SVzNL3DPoint>& inliers,
@ -365,4 +398,19 @@ SG_APISHARED_EXPORT void sg_pointClustering(
std::vector< SVzNL3DPosition>& pts,
double clusterDist,
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 <SG_errCode.h>
#define PI 3.141592654
#define PI 3.14159265358979323846
// 定义欧拉角结构体（单位：度）
typedef struct{
@ -199,6 +199,22 @@ typedef struct
double bagH; //高
}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
{
ESG_poseSortingMode sortMode;
@ -267,6 +283,15 @@ typedef struct
int value;
}SSG_RUN;
typedef struct
{
int start;
int len;
int value;
bool start_zRising; //起点z上跳标志
bool end_zRising;//终点z上跳标志
}SSG_RUN_EX;
typedef struct
{
double mean;
@ -414,3 +439,11 @@ typedef struct
double forward_z;
double backward_z;
}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;

185
sourceCode/SG_baseFunc.cpp
View File

@ -5,6 +5,7 @@
#include <cmath>
#include <unordered_map>
//计算扫描ROI
SVzNL3DRangeD sg_getScanDataROI(
SVzNL3DLaserLine* laser3DPoints,
int lineNum)
@ -65,6 +66,7 @@ SVzNL3DRangeD sg_getScanDataROI(
return roi;
}
//计算扫描ROI: vecotr格式
SVzNL3DRangeD sg_getScanDataROI_vector(std::vector< std::vector<SVzNL3DPosition>>& scanLines)
{
SVzNL3DRangeD roi;
@ -125,6 +127,93 @@ SVzNL3DRangeD sg_getScanDataROI_vector(std::vector< std::vector<SVzNL3DPosition>
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)
{
//最小二乘拟合直线参数
@ -1979,4 +2068,98 @@ void lineDataRT_RGBD(SVzNLXYZRGBDLaserLine* a_line, const double* camPoseR, doub
a_line->p3DPoint[i] = a_pt;
}
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;
}
}
}
}

5
sourceCode/SG_errCode.h
View File

@ -11,4 +11,7 @@
#define SX_ERR_INVLD_VTREE_NUM -2001
#define SX_ERR_INVLD_HTREE_NUM -2002
#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;
}
//将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(
SSG_basicFeature1D& a_feature,
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(
int lineIdx,
bool isLastLine,
@ -811,7 +931,6 @@ if (edge1_pts[i].x == 622)
#endif
}
void sg_getPeakGrowingTrees(
std::vector<std::vector< SSG_basicFeature1D>>& peakFeatures,
std::vector<SSG_featureTree>& trees,

400
sourceCode/SG_lineFeature.cpp
View File

@ -261,6 +261,62 @@ void sg_lineDataRemoveOutlier_changeOriginData(
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)
{
@ -1134,6 +1190,7 @@ void sg_getLineLVFeature(SVzNL3DPosition* lineData, int dataSize, int lineIdx, c
/// <summary>
/// 提取激光线上的拐点特征
/// seg端点：z距离大于门限
/// nPointIdx被重新定义成Feature类型
/// 算法流程：
/// （1）逐点计算前向角和后向角
@ -1445,6 +1502,7 @@ void sg_getLineCornerFeature(
/// <summary>
/// 提取激光线上的拐点特征，水平端点视为Corner
/// seg端点：z距离或y距离大于门限
/// 根据Seg进行corner提取
/// nPointIdx被重新定义成Feature类型
/// 算法流程：
@ -1787,11 +1845,351 @@ void sg_getLineCornerFeature_BQ(
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) {
return a.pntIdx < b.pntIdx;
}
/// <summary>
/// 提取激光线上的Jumping特征
/// 提取激光线上的Jumping特征, 用于搭接焊缝
/// seg端点：z距离大于门限
/// nPointIdx被重新定义成Feature类型
/// 算法流程：
/// （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;
}