更工件算法
This commit is contained in:
parent
908145f028
commit
ceb2f76484
@ -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];
|
||||
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);
|
||||
|
||||
|
||||
@ -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_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");
|
||||
|
||||
@ -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
|
||||
|
||||
@ -2871,8 +2871,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;
|
||||
@ -3039,7 +3039,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;
|
||||
|
||||
@ -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">
|
||||
|
||||
@ -5,6 +5,12 @@
|
||||
#include <opencv2/opencv.hpp>
|
||||
#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];
|
||||
}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(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||||
const SSG_cornerParam cornerPara,
|
||||
@ -179,6 +206,440 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
|
||||
int linePtNum = (int)scanLines[0].size();
|
||||
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;
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
@ -369,7 +830,7 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
|
||||
}
|
||||
int hvTreeSize = hvTreeIdx;
|
||||
|
||||
if(v_trees.size() < 2)
|
||||
if (v_trees.size() < 2)
|
||||
{
|
||||
*errCode = SX_ERR_INVLD_VTREE_NUM;
|
||||
return workpieceCorners;
|
||||
@ -403,9 +864,8 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
|
||||
hTree_R = i;
|
||||
}
|
||||
|
||||
SSX_featureContour region[4];
|
||||
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);
|
||||
//寻找对应的两边
|
||||
SVzNL3DPoint firstPt = region[0].edge[0];
|
||||
SVzNL3DPoint lastPt = region[0].edge.back();
|
||||
@ -472,6 +932,11 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
|
||||
return workpieceCorners;
|
||||
}
|
||||
_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
|
||||
_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;
|
||||
return workpieceCorners;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
for (int i = 0; i < 4; i++)
|
||||
{
|
||||
@ -642,6 +1109,7 @@ SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
|
||||
|
||||
|
||||
|
||||
|
||||
#if 1
|
||||
//将数据重新投射回原来的坐标系，以保持手眼标定结果正确
|
||||
for (int i = 0; i < lineNum; i++)
|
||||
|
||||
@ -47,6 +47,9 @@ typedef struct
|
||||
SVzNL3DPoint edge_link2_ends[2];
|
||||
}SSX_debugInfo;
|
||||
|
||||
//读版本号
|
||||
SG_WORKPIECESHARED_EXPORT const char* wd_BQWorkpieceCornerVersion(void);
|
||||
|
||||
//计算一个平面调平参数。
|
||||
//数据输入中可以有一个地平面和参考调平平面，以最高的平面进行调平
|
||||
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
|
||||
|
||||
@ -31,7 +31,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,
|
||||
@ -39,6 +42,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,
|
||||
@ -77,6 +81,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类型
|
||||
@ -142,6 +164,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,
|
||||
@ -290,6 +319,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,
|
||||
@ -375,3 +408,18 @@ SG_APISHARED_EXPORT void sg_pointClustering(
|
||||
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);
|
||||
@ -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;
|
||||
|
||||
@ -7,6 +7,7 @@
|
||||
#include <cmath>
|
||||
#include <unordered_map>
|
||||
|
||||
//计算扫描ROI
|
||||
SVzNL3DRangeD sg_getScanDataROI(
|
||||
SVzNL3DLaserLine* laser3DPoints,
|
||||
int lineNum)
|
||||
@ -67,6 +68,7 @@ SVzNL3DRangeD sg_getScanDataROI(
|
||||
return roi;
|
||||
}
|
||||
|
||||
//计算扫描ROI: vecotr格式
|
||||
SVzNL3DRangeD sg_getScanDataROI_vector(std::vector< std::vector<SVzNL3DPosition>>& scanLines)
|
||||
{
|
||||
SVzNL3DRangeD roi;
|
||||
@ -127,6 +129,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)
|
||||
{
|
||||
//最小二乘拟合直线参数
|
||||
@ -1982,3 +2071,97 @@ void lineDataRT_RGBD(SVzNLXYZRGBDLaserLine* a_line, const double* camPoseR, doub
|
||||
}
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -12,3 +12,6 @@
|
||||
#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_ZERO_CONTOUR_PT -2005
|
||||
#define SX_ERR_INVLID_RPEAK_NUM -2006
|
||||
#define SX_ERR_INVLID_RPEAK_PAIR -2007
|
||||
@ -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,
|
||||
|
||||
@ -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）逐点计算前向角和后向角
|
||||
|
||||
@ -1359,4 +1359,3 @@ void sg_getLocalPeaks_distTransform(cv::Mat& input, std::vector<SSG_2DValueI>& p
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user