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

rodAarcFeatueDetection version 1.1.0 :

Browse Source 
添加了地面调平和棒材定位
This commit is contained in:
jerryzeng 2026-03-21 23:15:43 +08:00
parent 0231f54828
commit 4bd528ebb9
10 changed files with 1422 additions and 245 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
beltTearingDetection_test/beltTearingDetection_test.vcxproj
View File

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

4
rodAndBarDetection/rodAndBarDetection.vcxproj
View File

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

316
rodAndBarDetection_test/rodAndBarDetection_test.cpp
View File

@ -165,6 +165,49 @@ void _outputScanDataFile(char* fileName, std::vector<std::vector< SVzNL3DPositio
sw.close();
}
SSG_planeCalibPara _readCalibPara(char* fileName)
{
//设置初始结果
double initCalib[9] = {
1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0 };
SSG_planeCalibPara planePara;
for (int i = 0; i < 9; i++)
planePara.planeCalib[i] = initCalib[i];
planePara.planeHeight = -1.0;
for (int i = 0; i < 9; i++)
planePara.invRMatrix[i] = initCalib[i];
std::ifstream inputFile(fileName);
std::string linedata;
if (inputFile.is_open() == false)
return planePara;
//调平矩阵
std::getline(inputFile, linedata);
sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.planeCalib[0], &planePara.planeCalib[1], &planePara.planeCalib[2]);
std::getline(inputFile, linedata);
sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.planeCalib[3], &planePara.planeCalib[4], &planePara.planeCalib[5]);
std::getline(inputFile, linedata);
sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.planeCalib[6], &planePara.planeCalib[7], &planePara.planeCalib[8]);
//地面高度
std::getline(inputFile, linedata);
sscanf_s(linedata.c_str(), "%lf", &planePara.planeHeight);
//反向旋转矩阵
std::getline(inputFile, linedata);
sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.invRMatrix[0], &planePara.invRMatrix[1], &planePara.invRMatrix[2]);
std::getline(inputFile, linedata);
sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.invRMatrix[3], &planePara.invRMatrix[4], &planePara.invRMatrix[5]);
std::getline(inputFile, linedata);
sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.invRMatrix[6], &planePara.invRMatrix[7], &planePara.invRMatrix[8]);
inputFile.close();
return planePara;
}
void _outputChanneltInfo(char* fileName, std::vector<SSX_hexHeadScrewInfo>& screwInfo)
{
std::ofstream sw(fileName);
@ -181,6 +224,23 @@ void _outputChanneltInfo(char* fileName, std::vector<SSX_hexHeadScrewInfo>& scre
sw.close();
}
void _outputRodtInfo(char* fileName, std::vector<SSX_rodPositionInfo>& rodInfo)
{
std::ofstream sw(fileName);
char dataStr[250];
int objNum = (int)rodInfo.size();
for (int i = 0; i < objNum; i++)
{
sprintf_s(dataStr, 250, "螺杆_%d: center_( %g, %g, %g ), dir_axia( %g, %g, %g ), dir_normal_( %g, %g, %g )",
i + 1, rodInfo[i].center.x, rodInfo[i].center.y, rodInfo[i].center.z,
rodInfo[i].axialDir.x, rodInfo[i].axialDir.y, rodInfo[i].axialDir.z,
rodInfo[i].normalDir.x, rodInfo[i].normalDir.y, rodInfo[i].normalDir.z);
sw << dataStr << std::endl;
}
sw.close();
}
void _outputRGBDScan_RGBD(
char* fileName,
std::vector<std::vector<SVzNL3DPosition>>& scanLines,
@ -317,21 +377,161 @@ void _outputRGBDScan_RGBD(
sw.close();
}
#define TEST_GROUP 1
int main()
void _outputRGBDScan_RGBD_rodInfo(
char* fileName,
std::vector<std::vector<SVzNL3DPosition>>& scanLines,
std::vector<SSX_rodPositionInfo>& rodInfo
)
{
const char* dataPath[TEST_GROUP] = {
int lineNum = (int)scanLines.size();
std::ofstream sw(fileName);
int realLines = lineNum;
int objNum = (int)rodInfo.size();
if (objNum > 0)
realLines += 1;
sw << "LineNum:" << realLines << std::endl;
sw << "DataType: 0" << std::endl;
sw << "ScanSpeed: 0" << std::endl;
sw << "PointAdjust: 1" << std::endl;
sw << "MaxTimeStamp: 0_0" << std::endl;
int maxLineIndex = 0;
int max_stamp = 0;
SG_color rgb = { 0, 0, 0 };
SG_color objColor[8] = {
{245,222,179},//淡黄色
{210,105, 30},//巧克力色
{240,230,140},//黄褐色
{135,206,235},//天蓝色
{250,235,215},//古董白
{189,252,201},//薄荷色
{221,160,221},//梅红色
{188,143,143},//玫瑰红色
};
int size = 1;
int lineIdx = 0;
for (int line = 0; line < lineNum; line++)
{
int linePtNum = (int)scanLines[line].size();
if (linePtNum == 0)
continue;
sw << "Line_" << lineIdx << "_0_" << linePtNum << std::endl;
lineIdx++;
for (int i = 0; i < linePtNum; i++)
{
SVzNL3DPosition* pt3D = &scanLines[line][i];
if (pt3D->nPointIdx > 0)
{
int centerFlag = pt3D->nPointIdx >> 4;
if (centerFlag > 0)
{
rgb = { 250, 0, 0 };
size = 6;
}
else
{
rgb = objColor[pt3D->nPointIdx % 8];
size = 3;
}
}
else //if (pt3D->nPointIdx == 0)
{
rgb = { 200, 200, 200 };
size = 1;
}
float x = (float)pt3D->pt3D.x;
float y = (float)pt3D->pt3D.y;
float z = (float)pt3D->pt3D.z;
sw << "{" << x << "," << y << "," << z << "}-";
sw << "{0,0}-{0,0}-";
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
}
}
if (objNum > 0)
{
sw << "Line_" << lineIdx << "_0_" << objNum << std::endl;
rgb = { 250, 0, 0 };
size = 8;
for (int i = 0; i < objNum; i++)
{
float x = (float)rodInfo[i].center.x;
float y = (float)rodInfo[i].center.y;
float z = (float)rodInfo[i].center.z;
sw << "{" << x << "," << y << "," << z << "}-";
sw << "{0,0}-{0,0}-";
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
}
//多输出一个，修正显示工具bug
float x = (float)rodInfo[0].center.x;
float y = (float)rodInfo[0].center.y;
float z = (float)rodInfo[0].center.z;
sw << "{" << x << "," << y << "," << z << "}-";
sw << "{0,0}-{0,0}-";
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
//输出法向
size = 1;
double len = 60;
lineIdx = 0;
for (int i = 0; i < objNum; i++)
{
SVzNL3DPoint pt0 = { rodInfo[i].center.x - len * rodInfo[i].axialDir.x,
rodInfo[i].center.y - len * rodInfo[i].axialDir.y,
rodInfo[i].center.z - len * rodInfo[i].axialDir.z };
SVzNL3DPoint pt1 = { rodInfo[i].center.x + len * rodInfo[i].axialDir.x,
rodInfo[i].center.y + len * rodInfo[i].axialDir.y,
rodInfo[i].center.z + len * rodInfo[i].axialDir.z };
//显示法向量
sw << "Poly_" << lineIdx << "_2" << std::endl;
sw << "{" << (float)pt0.x << "," << (float)pt0.y << "," << (float)pt0.z << "}-";
sw << "{0,0}-{0,0}-";
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
sw << "{" << pt1.x << "," << pt1.y << "," << pt1.z << "}-";
sw << "{0,0}-{0,0}-";
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
lineIdx++;
}
//多输出一个，修正显示工具bug
SVzNL3DPoint pt0 = { rodInfo[0].center.x - len * rodInfo[0].axialDir.x,
rodInfo[0].center.y - len * rodInfo[0].axialDir.y,
rodInfo[0].center.z - len * rodInfo[0].axialDir.z };
SVzNL3DPoint pt1 = { rodInfo[0].center.x + len * rodInfo[0].axialDir.x,
rodInfo[0].center.y + len * rodInfo[0].axialDir.y,
rodInfo[0].center.z + len * rodInfo[0].axialDir.z };
//显示法向量
sw << "Poly_" << lineIdx << "_2" << std::endl;
sw << "{" << (float)pt0.x << "," << (float)pt0.y << "," << (float)pt0.z << "}-";
sw << "{0,0}-{0,0}-";
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
sw << "{" << pt1.x << "," << pt1.y << "," << pt1.z << "}-";
sw << "{0,0}-{0,0}-";
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
lineIdx++;
}
sw.close();
}
#define SCREW_TEST_GROUP 1
void screwTest(void)
{
const char* dataPath[SCREW_TEST_GROUP] = {
"F:/ShangGu/项目/冠钦项目/螺杆测量/数据/模拟数据/", //0
};
SVzNLRange fileIdx[TEST_GROUP] = {
SVzNLRange fileIdx[SCREW_TEST_GROUP] = {
{1,4},
};
const char* ver = wd_rodAndBarDetectionVersion();
printf("ver:%s\n", ver);
for (int grp = 0; grp < TEST_GROUP; grp++)
for (int grp = 0; grp < SCREW_TEST_GROUP; grp++)
{
for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
{
@ -392,7 +592,113 @@ int main()
_outputChanneltInfo(_scan_file, screwInfo);
}
}
}
#define ROD_POSITION_TEST_GROUP 1
void rodPositionTest(void)
{
const char* dataPath[ROD_POSITION_TEST_GROUP] = {
"F:/ShangGu/项目/冠钦项目/棒材抓取/", //0
};
SVzNLRange fileIdx[ROD_POSITION_TEST_GROUP] = {
{1,8},
};
const char* ver = wd_rodAndBarDetectionVersion();
printf("ver:%s\n", ver);
for (int grp = 0; grp < ROD_POSITION_TEST_GROUP; grp++)
{
SSG_planeCalibPara poseCalibPara;
//初始化成单位阵
poseCalibPara.planeCalib[0] = 1.0;
poseCalibPara.planeCalib[1] = 0.0;
poseCalibPara.planeCalib[2] = 0.0;
poseCalibPara.planeCalib[3] = 0.0;
poseCalibPara.planeCalib[4] = 1.0;
poseCalibPara.planeCalib[5] = 0.0;
poseCalibPara.planeCalib[6] = 0.0;
poseCalibPara.planeCalib[7] = 0.0;
poseCalibPara.planeCalib[8] = 1.0;
poseCalibPara.planeHeight = -1.0;
for (int i = 0; i < 9; i++)
poseCalibPara.invRMatrix[i] = poseCalibPara.planeCalib[i];
//char calibFile[250];
//sprintf_s(calibFile, "%sground_calib_para.txt", dataPath[grp]);
//poseCalibPara = _readCalibPara(calibFile);
for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
{
fidx =1;
char _scan_file[256];
sprintf_s(_scan_file, "%sLaserData_%d.txt", dataPath[grp], fidx);
std::vector<std::vector< SVzNL3DPosition>> scanLines;
wdReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines);
//转成plyTxt格式
//sprintf_s(_scan_file, "%s%d_ply_Hi229229.txt", dataPath[grp], fidx);
//wdSavePlyTxt(_scan_file, scanLines);
long t1 = (long)GetTickCount64();//统计时间
SSX_rodParam rodParam;
rodParam.diameter = 52.0; //圆棒直径
rodParam.len = 290;
SSG_cornerParam cornerParam;
cornerParam.cornerTh = 60; //45度角
cornerParam.scale = rodParam.diameter / 4; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8;
cornerParam.minEndingGap = 20; // algoParam.bagParam.bagW / 4;
cornerParam.minEndingGap_z = 5.0;
cornerParam.jumpCornerTh_1 = 15; //水平角度，小于此角度视为水平
cornerParam.jumpCornerTh_2 = 60;
SSG_outlierFilterParam filterParam;
filterParam.continuityTh = 5.0; //噪声滤除。当相邻点的z跳变大于此门限时，检查是否为噪声。若长度小于outlierLen， 视为噪声
filterParam.outlierTh = 5;
SSG_treeGrowParam growParam;
growParam.maxLineSkipNum = 5;
growParam.yDeviation_max = 10.0;
growParam.maxSkipDistance = 10.0;
growParam.zDeviation_max = 10.0;//
growParam.minLTypeTreeLen = 100; //mm, 螺杆长度
growParam.minVTypeTreeLen = 100; //mm
bool isHorizonScan = true; //true:激光线平行槽道；false:激光线垂直槽道
int errCode = 0;
std::vector<SSX_rodPositionInfo> rodInfo;
sx_rodPositioning(
scanLines,
poseCalibPara,
cornerParam,
filterParam,
growParam,
rodParam,
rodInfo,
&errCode);
long t2 = (long)GetTickCount64();
printf("%s: %d(ms)!\n", _scan_file, (int)(t2 - t1));
//输出测试结果
sprintf_s(_scan_file, "%sresult\\%d_result.txt", dataPath[grp], fidx);
_outputRGBDScan_RGBD_rodInfo(_scan_file, scanLines, rodInfo);
sprintf_s(_scan_file, "%sresult\\%d_screw_info.txt", dataPath[grp], fidx);
_outputRodtInfo(_scan_file, rodInfo);
}
}
}
int main()
{
#if 0 //螺杆定位测试
screwTest();
#else //棒材抓取定位测试
rodPositionTest();
#endif
return 0;
}
// 运行程序: Ctrl + F5 或调试 >“开始执行(不调试)”菜单

4
rodAndBarDetection_test/rodAndBarDetection_test.vcxproj
View File

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

21
sourceCode/SG_baseAlgo_Export.h
View File

@ -149,6 +149,14 @@ SG_APISHARED_EXPORT void wd_getRingArcFeature(
SVzNLRangeD ringArcWidth, //定子的环宽度
std::vector<SWD_segFeature>& line_ringArcs //环
);
//检测棒材特征。根据棒材Arc的顶点正好是Z极小值特征，检测Z极小值处的拐角情况
SG_APISHARED_EXPORT void wd_getRodArcFeature_peakCornerMethod(
std::vector< SVzNL3DPosition>& lineData,
int lineIdx,
const double peakChkWin,
const SSG_cornerParam cornerPara,
std::vector<SWD_rodArcFeature>& line_rodArcs //环
);
/// <summary>
/// 提取激光线上的拐点特征。是在PSM， LVTypeFeature, BQ等拐点算法的基础上的版本。
@ -269,6 +277,13 @@ SG_APISHARED_EXPORT void sg_getLineLocalPeaks(
std::vector< SSG_basicFeature1D>& localMax,
std::vector< SSG_basicFeature1D>& localMin);
SG_APISHARED_EXPORT void sg_getLineLocalPeaks_2(
std::vector<SVzNL3DPosition>& lineData,
int lineIdx,
const double scaleWin,
std::vector< SSG_basicFeature1D>& localZMax,
std::vector< SSG_basicFeature1D>& localZMin);
SG_APISHARED_EXPORT void sg_getFlatLineLocalPeaks_vector(
std::vector<SVzNL3DPosition>& lineData,
int lineIdx,
@ -303,6 +318,12 @@ SG_APISHARED_EXPORT void wd_getSegFeatureGrowingTrees(
std::vector<SWD_segFeatureTree>& feature_trees,
SSG_treeGrowParam growParam);
//对rodArcfeature进行生长
SG_APISHARED_EXPORT void wd_getRodArcFeatureGrowingTrees(
std::vector<std::vector<SWD_rodArcFeature>>& all_lineFeatures,
std::vector<SWD_rodArcFeatureTree>& feature_trees,
const SSG_treeGrowParam growParam);
SG_APISHARED_EXPORT void sg_lineFeaturesGrowing(
int lineIdx,
bool isLastLine,

23
sourceCode/SG_baseDataType.h
View File

@ -257,6 +257,18 @@ typedef struct
double featureValue;
}SWD_segFeature;
typedef struct
{
int lineIdx;
int startPtIdx;
int endPtIdx;
int peakPtIdx;
SVzNL3DPoint startPt;
SVzNL3DPoint endPt;
SVzNL3DPoint peakPt;
double featureValue;
}SWD_rodArcFeature;
typedef struct
{
double sameGapTh;
@ -352,6 +364,15 @@ typedef struct
std::vector< SWD_segFeature> treeNodes;
}SWD_segFeatureTree;
typedef struct
{
int treeState;
int sLineIdx;
int eLineIdx;
double tree_value;
std::vector< SWD_rodArcFeature> treeNodes;
}SWD_rodArcFeatureTree;
typedef struct
{
int vTreeFlag;
@ -549,6 +570,8 @@ typedef struct
typedef struct
{
int pntIdx;
int forwardPntIdx;
int backwardPntIdx;
int type;
double forwardAngle; //Ç°Ïò½Ç
double backwardAngle; //ºóÏò½Ç

102
sourceCode/SG_featureGrow.cpp
View File

@ -146,6 +146,36 @@ bool _segFeatureGrowing(SWD_segFeature& a_feature, const int lineIdx, std::vecto
return false;
}
//将segment feature在trees上寻找合适的生长点进行生长。如果没有合适的生长点， 返回false
bool _rodArcFeatureGrowing(SWD_rodArcFeature& a_feature, const int lineIdx, std::vector<SWD_rodArcFeatureTree>& trees, const SSG_treeGrowParam growParam)
{
for (int i = 0, i_max = (int)trees.size(); i < i_max; i++)
{
SWD_rodArcFeatureTree& a_tree = trees[i];
if (TREE_STATE_DEAD == a_tree.treeState)
continue;
//检查生长点
SWD_rodArcFeature last_node = a_tree.treeNodes.back();
if (last_node.lineIdx == a_feature.lineIdx) //x为lineIdx，同一条扫描线上的不进行生长
continue;
//判断生长点
double y_diff_1 = abs(a_feature.peakPt.y - last_node.peakPt.y);
double z_diff_1 = abs(a_feature.peakPt.z - last_node.peakPt.z);
int line_diff = abs(a_feature.lineIdx - last_node.lineIdx);
double x_diff = abs(a_feature.peakPt.x - last_node.peakPt.x);
if ((y_diff_1 < growParam.yDeviation_max) && (z_diff_1 < 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.startPt.z + a_feature.endPt.z) / 2;
return true;
}
}
return false;
}
int _getMatchedTree_angleCheck(
SSG_basicFeature1D& a_feature,
const int lineIdx,
@ -638,7 +668,7 @@ void wd_getFeatureGrowingTrees_noTypeMatch(
void wd_getSegFeatureGrowingTrees(
std::vector<std::vector<SWD_segFeature>>& all_lineFeatures,
std::vector<SWD_segFeatureTree>& feature_trees,
SSG_treeGrowParam growParam)
const SSG_treeGrowParam growParam)
{
for (int i = 0, i_max = (int)all_lineFeatures.size(); i < i_max; i++)
{
@ -702,6 +732,76 @@ void wd_getSegFeatureGrowingTrees(
}
void wd_getRodArcFeatureGrowingTrees(
std::vector<std::vector<SWD_rodArcFeature>>& all_lineFeatures,
std::vector<SWD_rodArcFeatureTree>& feature_trees,
SSG_treeGrowParam growParam)
{
for (int i = 0, i_max = (int)all_lineFeatures.size(); i < i_max; i++)
{
if (i == 424)
int kkk = 1;
std::vector<SWD_rodArcFeature>& a_lineFeatures = all_lineFeatures[i];
for (int j = 0, j_max = (int)a_lineFeatures.size(); j < j_max; j++)
{
SWD_rodArcFeature& a_feature = a_lineFeatures[j];
bool isMatched = _rodArcFeatureGrowing(a_feature, i, feature_trees, growParam);
if (false == isMatched)
{
//新的生长树
SWD_rodArcFeatureTree a_newTree;
a_newTree.treeNodes.push_back(a_feature);
a_newTree.treeState = TREE_STATE_ALIVE;
a_newTree.sLineIdx = a_feature.lineIdx;
a_newTree.eLineIdx = a_feature.lineIdx;
a_newTree.tree_value = (a_feature.startPt.z + a_feature.endPt.z) / 2;
feature_trees.push_back(a_newTree);
}
}
//检查停止生长的树,加速。
//将生长节点为1的生长树移除
int lineIdx = i;
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().lineIdx);
if (line_diff > 0)
{
SWD_rodArcFeature* first_node = &(feature_trees[m].treeNodes[0]);
SWD_rodArcFeature* last_node = &(feature_trees[m].treeNodes[feature_trees[m].treeNodes.size() - 1]);
double len = sqrt(pow(first_node->peakPt.x - last_node->peakPt.x, 2) +
pow(first_node->peakPt.y - last_node->peakPt.y, 2) +
pow(first_node->peakPt.z - last_node->peakPt.z, 2));
int tree_lines = feature_trees[m].eLineIdx - feature_trees[m].sLineIdx;
if (tree_lines > 0)
{
double mean_stepping = len / tree_lines;
double x_skip = mean_stepping * line_diff;
if (((growParam.maxLineSkipNum > 0) && (line_diff > growParam.maxLineSkipNum)) ||
((growParam.maxLineSkipNum <= 0) && (x_skip > growParam.maxSkipDistance)) ||
(i == i_max - 1))
{
feature_trees[m].treeState = TREE_STATE_DEAD;
if (len <= growParam.minLTypeTreeLen)
feature_trees.erase(feature_trees.begin() + m);
}
}
}
}
}
}
//将node为1的tree去除。生长时可能无法去除node数量为1的tree
int m_max = (int)feature_trees.size();
for (int m = m_max - 1; m >= 0; m--) //从后往前，这样删除不会影响循环
{
int tree_lines = feature_trees[m].eLineIdx - feature_trees[m].sLineIdx;
if (tree_lines == 0)
feature_trees.erase(feature_trees.begin() + m);
}
}
void sg_lineFeaturesGrowing(
int lineIdx,
bool isLastLine,

890
sourceCode/SG_lineFeature.cpp
View File

@ -443,6 +443,359 @@ void sg_lineDataRemoveOutlier_ptDistMethod(SVzNL3DPosition* lineData, int dataSi
return;
}
/// <summary>
/// 提取激光线上的极值点（极大值点和极小值点）
///
/// </summary>
void sg_getLineLocalPeaks(
SVzNL3DPosition* lineData,
int dataSize,
int lineIdx,
const double scaleWin,
std::vector< SSG_basicFeature1D>& localMax,
std::vector< SSG_basicFeature1D>& localMin)
{
if (dataSize < 2)
return;
int _state = 0;
int pre_i = -1;
int sEdgePtIdx = -1;
int eEdgePtIdx = -1;
SVzNL3DPosition* pre_data = NULL;
std::vector< SVzNL3DPosition> pkTop;
std::vector< SVzNL3DPosition> pkBtm;
for (int i = 0; i < dataSize; i++)
{
lineData[i].nPointIdx = i;
SVzNL3DPosition* curr_data = &lineData[i];
if (curr_data->pt3D.z < 1e-4)
{
if (i == dataSize - 1) //最后一个
{
if (1 == _state) //上升
{
pkTop.push_back(lineData[eEdgePtIdx]);
}
else if (2 == _state) //下降
{
pkBtm.push_back(lineData[eEdgePtIdx]);
}
}
continue;
}
if (NULL == pre_data)
{
sEdgePtIdx = i;
eEdgePtIdx = i;
pre_data = curr_data;
pre_i = i;
continue;
}
eEdgePtIdx = i;
double z_diff = curr_data->pt3D.z - pre_data->pt3D.z;
switch (_state)
{
case 0: //初态
if (z_diff > 0) //下降
{
_state = 2;
}
else if (z_diff < 0) //上升
{
_state = 1;
}
break;
case 1: //上升
if (z_diff > 0) //下降
{
pkTop.push_back(*pre_data);
_state = 2;
}
break;
case 2: //下降
if (z_diff < 0) // 上升
{
pkBtm.push_back(*pre_data);
_state = 1;
}
break;
default:
_state = 0;
break;
}
pre_data = curr_data;
pre_i = i;
}
//注意：最后一个不处理，为基座位置
//极小值点（峰顶）
//极值比较，在尺度窗口下寻找局部极值点
double square_distTh = scaleWin * scaleWin;
for (int i = 0, i_max = (int)pkTop.size(); i < i_max; i++)
{
bool isPeak = true;
//向前搜索
for (int j = i - 1; j >= 0; j--)
{
double dist = pow(pkTop[i].pt3D.y - pkTop[j].pt3D.y, 2); // + pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2) ;
if (dist > square_distTh) //超出尺度窗口
break;
if (pkTop[i].pt3D.z > pkTop[j].pt3D.z)
{
isPeak = false;
break;
}
}
//向后搜索
if (true == isPeak)
{
for (int j = i + 1; j < i_max; j++)
{
double dist = pow(pkTop[i].pt3D.y - pkTop[j].pt3D.y, 2); // +pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2);
if (dist > square_distTh) //超出尺度窗口
break;
if (pkTop[i].pt3D.z > pkTop[j].pt3D.z)
{
isPeak = false;
break;
}
}
}
if (true == isPeak)
{
SSG_basicFeature1D a_feature;
a_feature.featureType = LINE_FEATURE_PEAK_TOP;
a_feature.jumpPos = pkTop[i].pt3D;
a_feature.jumpPos2D = { lineIdx, pkTop[i].nPointIdx };
localMin.push_back(a_feature);
}
}
//极大值点（谷底）
for (int i = 0, i_max = (int)pkBtm.size(); i < i_max; i++)
{
bool isPeak = true;
//向前搜索
for (int j = i - 1; j >= 0; j--)
{
double dist = pow(pkBtm[i].pt3D.y - pkBtm[j].pt3D.y, 2); // +pow(pkBtm[i].pt3D.x - pkBtm[j].pt3D.x, 2);
if (dist > square_distTh) //超出尺度窗口
break;
if (pkBtm[i].pt3D.z < pkBtm[j].pt3D.z)
{
isPeak = false;
break;
}
}
//向后搜索
if (true == isPeak)
{
for (int j = i + 1; j < i_max; j++)
{
double dist = pow(pkBtm[i].pt3D.y - pkBtm[j].pt3D.y, 2); // +pow(pkBtm[i].pt3D.x - pkBtm[j].pt3D.x, 2);
if (dist > square_distTh) //超出尺度窗口
break;
if (pkBtm[i].pt3D.z < pkBtm[j].pt3D.z)
{
isPeak = false;
break;
}
}
}
if (true == isPeak)
{
SSG_basicFeature1D a_feature;
a_feature.featureType = LINE_FEATURE_PEAK_BOTTOM;
a_feature.jumpPos = pkBtm[i].pt3D;
a_feature.jumpPos2D = { lineIdx, pkBtm[i].nPointIdx };
localMax.push_back(a_feature);
}
}
return;
}
void sg_getLineLocalPeaks_2(
std::vector<SVzNL3DPosition>& lineData,
int lineIdx,
const double scaleWin,
std::vector< SSG_basicFeature1D>& localZMax,
std::vector< SSG_basicFeature1D>& localZMin)
{
int dataSize = (int)lineData.size();
if (dataSize < 2)
return;
int _state = 0;
int pre_i = -1;
int sEdgePtIdx = -1;
int eEdgePtIdx = -1;
SVzNL3DPosition* pre_data = NULL;
std::vector< SVzNL3DPosition> zTop;
std::vector< SVzNL3DPosition> zBtm;
for (int i = 0; i < dataSize; i++)
{
if (i == 355)
int kkk = 1;
lineData[i].nPointIdx = i;
SVzNL3DPosition* curr_data = &lineData[i];
if (curr_data->pt3D.z < 1e-4)
{
if (i == dataSize - 1) //最后一个
{
if (1 == _state) //上升
{
zTop.push_back(lineData[eEdgePtIdx]);
}
else if (2 == _state) //下降
{
zBtm.push_back(lineData[eEdgePtIdx]);
}
}
continue;
}
if (NULL == pre_data)
{
sEdgePtIdx = i;
eEdgePtIdx = i;
pre_data = curr_data;
pre_i = i;
continue;
}
eEdgePtIdx = i;
double z_diff = curr_data->pt3D.z - pre_data->pt3D.z;
switch (_state)
{
case 0: //初态
if (z_diff < 0) //下降
{
_state = 2;
}
else if (z_diff > 0) //上升
{
_state = 1;
}
break;
case 1: //上升
if (z_diff <0) //下降
{
zTop.push_back(*pre_data);
_state = 2;
}
break;
case 2: //下降
if (z_diff > 0) // 上升
{
zBtm.push_back(*pre_data);
_state = 1;
}
break;
default:
_state = 0;
break;
}
pre_data = curr_data;
pre_i = i;
}
//注意：最后一个不处理，为基座位置
//极小值点（峰顶）
//极值比较，在尺度窗口下寻找局部极值点
double square_distTh = scaleWin * scaleWin;
for (int i = 0, i_max = (int)zTop.size(); i < i_max; i++)
{
bool isPeak = true;
//向前搜索
for (int j = i - 1; j >= 0; j--)
{
double dist = pow(zTop[i].pt3D.y - zTop[j].pt3D.y, 2) + pow(zTop[i].pt3D.z - zTop[j].pt3D.z, 2) ;
if (dist > square_distTh) //超出尺度窗口
break;
if (zTop[i].pt3D.z < zTop[j].pt3D.z)
{
isPeak = false;
break;
}
}
//向后搜索
if (true == isPeak)
{
for (int j = i + 1; j < i_max; j++)
{
double dist = pow(zTop[i].pt3D.y - zTop[j].pt3D.y, 2) + pow(zTop[i].pt3D.z - zTop[j].pt3D.z, 2);
if (dist > square_distTh) //超出尺度窗口
break;
if (zTop[i].pt3D.z < zTop[j].pt3D.z)
{
isPeak = false;
break;
}
}
}
if (true == isPeak)
{
SSG_basicFeature1D a_feature;
a_feature.featureType = LINE_FEATURE_PEAK_TOP;
a_feature.jumpPos = zTop[i].pt3D;
a_feature.jumpPos2D = { lineIdx, zTop[i].nPointIdx };
localZMax.push_back(a_feature);
}
}
//极大值点（谷底）
for (int i = 0, i_max = (int)zBtm.size(); i < i_max; i++)
{
bool isPeak = true;
//向前搜索
for (int j = i - 1; j >= 0; j--)
{
double dist = pow(zBtm[i].pt3D.y - zBtm[j].pt3D.y, 2) + pow(zBtm[i].pt3D.z - zBtm[j].pt3D.z, 2);
if (dist > square_distTh) //超出尺度窗口
break;
if (zBtm[i].pt3D.z > zBtm[j].pt3D.z)
{
isPeak = false;
break;
}
}
//向后搜索
if (true == isPeak)
{
for (int j = i + 1; j < i_max; j++)
{
double dist = pow(zBtm[i].pt3D.y - zBtm[j].pt3D.y, 2); pow(zBtm[i].pt3D.z - zBtm[j].pt3D.z, 2);
if (dist > square_distTh) //超出尺度窗口
break;
if (zBtm[i].pt3D.z > zBtm[j].pt3D.z)
{
isPeak = false;
break;
}
}
}
if (true == isPeak)
{
SSG_basicFeature1D a_feature;
a_feature.featureType = LINE_FEATURE_PEAK_BOTTOM;
a_feature.jumpPos = zBtm[i].pt3D;
a_feature.jumpPos2D = { lineIdx, zBtm[i].nPointIdx };
localZMin.push_back(a_feature);
}
}
return;
}
/// <summary>
/// 获取扫描线中的斜坡，并记录斜坡中的最大的相邻点之间的跳变。
/// 同时记录Ending。当没有中间袋子，只有左右袋子时，会有两组Ending。Ending之间的最小间距设置为袋子宽度的1/4
@ -3429,32 +3782,17 @@ void wd_getLineCornerFeature_PSM(
bool compareByPtIdx(const SSG_basicFeature1D& a, const SSG_basicFeature1D& b) {
return a.jumpPos2D.y < b.jumpPos2D.y;
}
/// <summary>
/// 提取激光线上的圆环的上半段弧。宽度由圆环的宽度确定
/// seg端点：z距离大于门限
/// nPointIdx被重新定义成Feature类型
/// 算法流程：
/// （1）逐点计算前向角和后向角
/// （2）逐点计算拐角，顺时针为负，逆时针为正
/// （3）搜索正拐角的极大值。
/// （4）判断拐角是否为跳变
/// </summary>
void wd_getRingArcFeature(
//根据Z连续性分段
void _lineDataSegment_zDist(
std::vector< SVzNL3DPosition>& lineData,
int lineIdx,
const SSG_cornerParam cornerPara,
SVzNLRangeD ringArcWidth, //环宽度
std::vector<SWD_segFeature>& line_ringArcs //环
std::vector< SVzNL3DPosition>& vldPts,
std::vector<SSG_RUN_EX>& segs,
std::vector<int>& backIndexing,
const SSG_cornerParam cornerPara
)
{
int dataSize = (int)lineData.size();
//去除零点
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;
@ -3515,21 +3853,17 @@ void wd_getRingArcFeature(
}
if (a_run.len > 0)
segs.push_back(a_run);
}
#if 0
//将点置标志
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;
}
#endif
//计算前向角和后向角
std::vector< SSG_pntDirAngle> corners;
corners.resize(vldPts.size());
//逐段计算方向角
void _computeDirAngle_perSeg(
std::vector< SVzNL3DPosition>& vldPts,
std::vector<SSG_RUN_EX>& segs,
const SSG_cornerParam cornerPara,
std::vector< SSG_pntDirAngle>& ptDirAngles
)
{
ptDirAngles.resize(vldPts.size());
//逐段进行
int segSize = (int)segs.size();
for (int segIdx = 0; segIdx < segSize; segIdx++)
@ -3565,12 +3899,14 @@ void wd_getRingArcFeature(
//计算拐角
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;
ptDirAngles[i].pntIdx = -1;
ptDirAngles[i].forwardPntIdx = 0;
ptDirAngles[i].backwardPntIdx = 0;
ptDirAngles[i].forwardAngle = 0;
ptDirAngles[i].backwardAngle = 0;
ptDirAngles[i].corner = 0;
ptDirAngles[i].forwardDiffZ = 0;
ptDirAngles[i].backwardDiffZ = 0;
}
else
{
@ -3578,15 +3914,134 @@ void wd_getRingArcFeature(
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;
ptDirAngles[i].pntIdx = i;
ptDirAngles[i].forwardPntIdx = post_i;
ptDirAngles[i].backwardPntIdx = pre_i;
ptDirAngles[i].forwardAngle = forwardAngle;
ptDirAngles[i].backwardAngle = backwardAngle;
ptDirAngles[i].corner = -(forwardAngle - backwardAngle); //图像坐标系与正常坐标系y方向相反，所以有“-”号
ptDirAngles[i].forwardDiffZ = vldPts[post_i].pt3D.z - vldPts[i].pt3D.z;
ptDirAngles[i].backwardDiffZ = vldPts[i].pt3D.z - vldPts[pre_i].pt3D.z;
}
}
}
}
//计算方向角(不分段）
void _computeDirAngle_wholeLine(
std::vector< SVzNL3DPosition>& line_data,
const SSG_cornerParam cornerPara,
std::vector< SSG_pntDirAngle>& ptDirAngles
)
{
ptDirAngles.resize(line_data.size());
int dataSize = (int)line_data.size();
for (int i = 0; i < dataSize; i++)
{
if (line_data[i].pt3D.z < 1e-4)
{
ptDirAngles[i].pntIdx = i;
ptDirAngles[i].forwardPntIdx = 0;
ptDirAngles[i].backwardPntIdx = 0;
ptDirAngles[i].type = -1;
ptDirAngles[i].forwardAngle = 0;
ptDirAngles[i].backwardAngle = 0;
ptDirAngles[i].corner = 0;
ptDirAngles[i].forwardDiffZ = 0;
ptDirAngles[i].backwardDiffZ = 0;
continue;
}
//前向寻找
int pre_i = -1;
for (int j = i - 1; j >= 0; j--)
{
if (line_data[j].pt3D.z < 1e-4)
continue;
double dist = sqrt(pow(line_data[i].pt3D.y - line_data[j].pt3D.y, 2) +
pow(line_data[i].pt3D.z - line_data[j].pt3D.z, 2));
if (dist >= cornerPara.scale)
{
pre_i = j;
break;
}
}
//后向寻找
int post_i = -1;
for (int j = i + 1; j < dataSize; j++)
{
double dist = sqrt(pow(line_data[i].pt3D.y - line_data[j].pt3D.y, 2) +
pow(line_data[i].pt3D.z - line_data[j].pt3D.z, 2));
if (dist >= cornerPara.scale)
{
post_i = j;
break;
}
}
//计算拐角
if ((pre_i < 0) || (post_i < 0))
{
ptDirAngles[i].pntIdx = i;
ptDirAngles[i].type = -1;
ptDirAngles[i].forwardAngle = 0;
ptDirAngles[i].backwardAngle = 0;
ptDirAngles[i].forwardPntIdx = 0;
ptDirAngles[i].backwardPntIdx = 0;
ptDirAngles[i].corner = 0;
ptDirAngles[i].forwardDiffZ = 0;
ptDirAngles[i].backwardDiffZ = 0;
}
else
{
double tanValue_pre = (line_data[i].pt3D.z - line_data[pre_i].pt3D.z) / abs(line_data[i].pt3D.y - line_data[pre_i].pt3D.y);
double tanValue_post = (line_data[post_i].pt3D.z - line_data[i].pt3D.z) / abs(line_data[post_i].pt3D.y - line_data[i].pt3D.y);
double forwardAngle = atan(tanValue_post) * 180.0 / PI;
double backwardAngle = atan(tanValue_pre) * 180.0 / PI;
ptDirAngles[i].pntIdx = i;
ptDirAngles[i].forwardPntIdx = post_i;
ptDirAngles[i].backwardPntIdx = pre_i;
ptDirAngles[i].type = 0;
ptDirAngles[i].forwardAngle = forwardAngle;
ptDirAngles[i].backwardAngle = backwardAngle;
ptDirAngles[i].corner = -(forwardAngle - backwardAngle); //图像坐标系与正常坐标系y方向相反，所以有“-”号
ptDirAngles[i].forwardDiffZ = line_data[post_i].pt3D.z - line_data[i].pt3D.z;
ptDirAngles[i].backwardDiffZ = line_data[i].pt3D.z - line_data[pre_i].pt3D.z;
}
}
}
/// <summary>
/// 提取激光线上的圆环的上半段弧。宽度由圆环的宽度确定
/// seg端点：z距离大于门限
/// nPointIdx被重新定义成Feature类型
/// 算法流程：
/// （1）逐点计算前向角和后向角
/// （2）逐点计算拐角，顺时针为负，逆时针为正
/// （3）搜索正拐角的极大值。
/// （4）判断拐角是否为跳变
/// </summary>
void wd_getRingArcFeature(
std::vector< SVzNL3DPosition>& lineData,
int lineIdx,
const SSG_cornerParam cornerPara,
SVzNLRangeD ringArcWidth, //环宽度
std::vector<SWD_segFeature>& line_ringArcs //环
)
{
int dataSize = (int)lineData.size();
//去除零点
std::vector< SVzNL3DPosition> vldPts;
//std::vector< int> vldPtSegIdx;
std::vector<SSG_RUN_EX> segs;
std::vector<int> backIndexing;
backIndexing.resize(dataSize);
//根据z连续性分段
_lineDataSegment_zDist(lineData, vldPts, segs, backIndexing, cornerPara);
//计算前向角和后向角
std::vector< SSG_pntDirAngle> ptDirAngles;
_computeDirAngle_perSeg(vldPts, segs, cornerPara, ptDirAngles);
//逐段寻找连续的负corner段。Arc上的每一个点都是负的corner。
int vldPtSize = (int)vldPts.size();
@ -3594,25 +4049,26 @@ void wd_getRingArcFeature(
int endIdx = -1;
double startAngle = 0;
double endAngle = 0;
int segSize = (int)segs.size();
for (int segIdx = 0; segIdx < segSize; segIdx++)
{
int vPtIdxStart = segs[segIdx].start;
int vPtIdxEnd = vPtIdxStart + segs[segIdx].len - 1;
for (int i = vPtIdxStart; i <= vPtIdxEnd; i++)
{
if (corners[i].corner < 0)
if (ptDirAngles[i].corner < 0)
{
if (startIdx < 0)
{
startIdx = i;
endIdx = i;
startAngle = corners[i].backwardAngle;
endAngle = corners[i].forwardAngle;
startAngle = ptDirAngles[i].backwardAngle;
endAngle = ptDirAngles[i].forwardAngle;
}
else
{
endIdx = i;
endAngle = corners[i].forwardAngle;
endAngle = ptDirAngles[i].forwardAngle;
if (i == dataSize - 1)
{
double totalCorner = abs(endAngle - startAngle);
@ -3631,7 +4087,7 @@ void wd_getRingArcFeature(
SWD_segFeature a_ringFeautre;
memset(&a_ringFeautre, 0, sizeof(SWD_segFeature));
a_ringFeautre.lineIdx = lineIdx;
if ((corners[preIdx].pntIdx < 0) && (corners[postIdx].pntIdx < 0))
if ((ptDirAngles[preIdx].pntIdx < 0) && (ptDirAngles[postIdx].pntIdx < 0))
{
a_ringFeautre.startPt = vldPts[vPtIdxStart].pt3D;
a_ringFeautre.endPt = vldPts[vPtIdxEnd].pt3D;
@ -3671,7 +4127,7 @@ void wd_getRingArcFeature(
SWD_segFeature a_ringFeautre;
memset(&a_ringFeautre, 0, sizeof(SWD_segFeature));
a_ringFeautre.lineIdx = lineIdx;
if ((corners[preIdx].pntIdx < 0) && (corners[postIdx].pntIdx < 0))
if ((ptDirAngles[preIdx].pntIdx < 0) && (ptDirAngles[postIdx].pntIdx < 0))
{
a_ringFeautre.startPt = vldPts[vPtIdxStart].pt3D;
a_ringFeautre.endPt = vldPts[vPtIdxEnd].pt3D;
@ -3700,6 +4156,154 @@ void wd_getRingArcFeature(
return;
}
//此处考虑到ZPeak会有波动，考虑到方向角计算是以尺度为基准，以方向角为准
bool _chkRodArcFeature(
int pkIdx, double searchWin,
std::vector< SSG_pntDirAngle>& ptDirAngles,
std::vector< SVzNL3DPosition>& lineData,
const double arcPerPointCornerMinValue, //arc上每个点的转角最小值
const double arcTotalCornerMinValue, //整个Arc的转角最小值
SSG_intPair& a_rodPos)
{
//pkIdx可能会有波动
double realPkCorner = abs(ptDirAngles[pkIdx].forwardAngle + ptDirAngles[pkIdx].backwardAngle);
int realPkIdx = pkIdx;
//向前搜索
for (int i = pkIdx - 1; i >= 0; i--)
{
if (lineData[i].pt3D.z < 1e-4)
continue;
double dist = sqrt(pow(lineData[pkIdx].pt3D.y - lineData[i].pt3D.y, 2) + pow(lineData[pkIdx].pt3D.z - lineData[i].pt3D.z, 2));
if (dist > searchWin)
break;
double flatAngle = abs(ptDirAngles[i].forwardAngle + ptDirAngles[i].backwardAngle);
if (realPkCorner > flatAngle)
{
realPkCorner = flatAngle;
realPkIdx = i;
}
}
//向后搜索
for (int i = pkIdx + 1; i < (int)lineData.size(); i++)
{
if (lineData[i].pt3D.z < 1e-4)
continue;
double dist = sqrt(pow(lineData[pkIdx].pt3D.y - lineData[i].pt3D.y, 2) + pow(lineData[pkIdx].pt3D.z - lineData[i].pt3D.z, 2));
if (dist > searchWin)
break;
double flatAngle = abs(ptDirAngles[i].forwardAngle + ptDirAngles[i].backwardAngle);
if (realPkCorner > flatAngle)
{
realPkCorner = flatAngle;
realPkIdx = i;
}
}
if ((ptDirAngles[realPkIdx].forwardAngle > 0) && (ptDirAngles[realPkIdx].backwardAngle < 0) &&
(realPkCorner <10) &&(ptDirAngles[realPkIdx].corner < -arcPerPointCornerMinValue)) //前向角为正，后向角为负，两者值基本相等（<10), 拐角为负，绝对值大于10度
{
//向前搜索
int pre_idx = -1;
for (int i = realPkIdx - 1; i >= 0; i--)
{
if (ptDirAngles[i].type < 0)
continue;
if (ptDirAngles[i].corner < -arcPerPointCornerMinValue)
pre_idx = i;
else
break;
}
//向后搜索
int post_idx = -1;
for (int i = realPkIdx + 1; i < (int)ptDirAngles.size(); i++)
{
if (ptDirAngles[i].type < 0)
continue;
if (ptDirAngles[i].corner < -arcPerPointCornerMinValue)
post_idx = i;
else
break;
}
if ((pre_idx >= 0) && (post_idx >= 0))
{
double totalCornerAngle = ptDirAngles[post_idx].forwardAngle - ptDirAngles[pre_idx].backwardAngle;
if (totalCornerAngle > arcTotalCornerMinValue) //整个圆弧需要至少60度的扇区。
{
a_rodPos.idx = realPkIdx;
a_rodPos.data_0 = pre_idx;
a_rodPos.data_1 = post_idx;
return true;
}
}
}
return false;
}
/// <summary>
/// 提取激光线上的圆环的上半段弧。
/// seg端点：z距离大于门限
/// nPointIdx被重新定义成Feature类型
/// 算法流程：
/// （1）逐点计算前向角和后向角
/// （2）搜索同方向的拐角连续段
/// （2）搜索Z极值，
/// （2）逐点计算拐角，顺时针为负，逆时针为正
/// （3）搜索正拐角的极大值。
/// （4）判断拐角是否为跳变
///
/// </summary>
void wd_getRodArcFeature_peakCornerMethod(
std::vector< SVzNL3DPosition>& lineData,
int lineIdx,
const double peakChkWin,
const SSG_cornerParam cornerPara,
std::vector<SWD_rodArcFeature>& line_rodArcs //环
)
{
double arcPerPointCornerMinValue = 5; //arc上每个点的转角最小值
double arcTotalCornerMinValue = 45; //整个Arc的转角最小值
//计算前向角和后向角
std::vector< SSG_pntDirAngle> ptDirAngles;
_computeDirAngle_wholeLine( lineData, cornerPara, ptDirAngles);
int dataSize = (int)lineData.size();
//搜索z极值。
std::vector< SSG_basicFeature1D> localZMax;
std::vector< SSG_basicFeature1D> localZMin;
sg_getLineLocalPeaks_2( lineData, lineIdx, peakChkWin, localZMax, localZMin);
//逐个极小值点判断
int peakNum = (int)localZMin.size();
for (int i = 0; i < peakNum; i++)
{
int pkIdx = localZMin[i].jumpPos2D.y;
SSG_intPair a_rodPos;
bool isArc = _chkRodArcFeature(
pkIdx, cornerPara.scale,
ptDirAngles, lineData,
arcPerPointCornerMinValue, arcTotalCornerMinValue,
a_rodPos);
if (true == isArc)
{
SWD_rodArcFeature a_rodArc;
memset(&a_rodArc, 0, sizeof(SWD_rodArcFeature));
a_rodArc.lineIdx = lineIdx;
a_rodArc.startPtIdx = a_rodPos.data_0;
a_rodArc.endPtIdx = a_rodPos.data_1;
a_rodArc.peakPtIdx = a_rodPos.idx;
a_rodArc.startPt = lineData[a_rodPos.data_0].pt3D;
a_rodArc.endPt = lineData[a_rodPos.data_1].pt3D;
a_rodArc.peakPt = lineData[a_rodPos.idx].pt3D;
a_rodArc.featureValue = ptDirAngles[a_rodPos.idx].corner;
line_rodArcs.push_back(a_rodArc);
}
}
return;
}
//提取corner极值（较早实现函数可以使用此函数进行代码优化）
void _searchCornerPeaks(
std::vector< SSG_pntDirAngle>& corners,
@ -5463,184 +6067,6 @@ void wd_getLineGloveArcs(
return;
}
/// <summary>
/// 提取激光线上的极值点（极大值点和极小值点）
///
/// </summary>
void sg_getLineLocalPeaks(
SVzNL3DPosition* lineData,
int dataSize,
int lineIdx,
const double scaleWin,
std::vector< SSG_basicFeature1D>& localMax,
std::vector< SSG_basicFeature1D>& localMin)
{
if (dataSize < 2)
return;
int _state = 0;
int pre_i = -1;
int sEdgePtIdx = -1;
int eEdgePtIdx = -1;
SVzNL3DPosition* pre_data = NULL;
std::vector< SVzNL3DPosition> pkTop;
std::vector< SVzNL3DPosition> pkBtm;
for (int i = 0; i < dataSize; i++)
{
lineData[i].nPointIdx = i;
SVzNL3DPosition* curr_data = &lineData[i];
if (curr_data->pt3D.z < 1e-4)
{
if (i == dataSize - 1) //最后一个
{
if ( 1 == _state ) //上升
{
pkTop.push_back(lineData[eEdgePtIdx]);
}
else if (2 == _state) //下降
{
pkBtm.push_back(lineData[eEdgePtIdx]);
}
}
continue;
}
if (NULL == pre_data)
{
sEdgePtIdx = i;
eEdgePtIdx = i;
pre_data = curr_data;
pre_i = i;
continue;
}
eEdgePtIdx = i;
double z_diff = curr_data->pt3D.z - pre_data->pt3D.z;
switch (_state)
{
case 0: //初态
if (z_diff > 0) //下降
{
_state = 2;
}
else if (z_diff < 0) //上升
{
_state = 1;
}
break;
case 1: //上升
if (z_diff > 0) //下降
{
pkTop.push_back(*pre_data);
_state = 2;
}
break;
case 2: //下降
if (z_diff < 0) // 上升
{
pkBtm.push_back(*pre_data);
_state = 1;
}
break;
default:
_state = 0;
break;
}
pre_data = curr_data;
pre_i = i;
}
//注意：最后一个不处理，为基座位置
//极小值点（峰顶）
//极值比较，在尺度窗口下寻找局部极值点
double square_distTh = scaleWin * scaleWin;
for (int i = 0, i_max = (int)pkTop.size(); i < i_max; i++)
{
bool isPeak = true;
//向前搜索
for (int j = i - 1; j >= 0; j--)
{
double dist = pow(pkTop[i].pt3D.y - pkTop[j].pt3D.y, 2); // + pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2) ;
if (dist > square_distTh) //超出尺度窗口
break;
if (pkTop[i].pt3D.z > pkTop[j].pt3D.z)
{
isPeak = false;
break;
}
}
//向后搜索
if (true == isPeak)
{
for (int j = i + 1; j < i_max; j++)
{
double dist = pow(pkTop[i].pt3D.y - pkTop[j].pt3D.y, 2); // +pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2);
if (dist > square_distTh) //超出尺度窗口
break;
if (pkTop[i].pt3D.z > pkTop[j].pt3D.z)
{
isPeak = false;
break;
}
}
}
if (true == isPeak)
{
SSG_basicFeature1D a_feature;
a_feature.featureType = LINE_FEATURE_PEAK_TOP;
a_feature.jumpPos = pkTop[i].pt3D;
a_feature.jumpPos2D = { lineIdx, pkTop[i].nPointIdx };
localMin.push_back(a_feature);
}
}
//极大值点（谷底）
for (int i = 0, i_max = (int)pkBtm.size(); i < i_max; i++)
{
bool isPeak = true;
//向前搜索
for (int j = i - 1; j >= 0; j--)
{
double dist = pow(pkBtm[i].pt3D.y - pkBtm[j].pt3D.y, 2); // +pow(pkBtm[i].pt3D.x - pkBtm[j].pt3D.x, 2);
if (dist > square_distTh) //超出尺度窗口
break;
if (pkBtm[i].pt3D.z < pkBtm[j].pt3D.z)
{
isPeak = false;
break;
}
}
//向后搜索
if (true == isPeak)
{
for (int j = i + 1; j < i_max; j++)
{
double dist = pow(pkBtm[i].pt3D.y - pkBtm[j].pt3D.y, 2); // +pow(pkBtm[i].pt3D.x - pkBtm[j].pt3D.x, 2);
if (dist > square_distTh) //超出尺度窗口
break;
if (pkBtm[i].pt3D.z < pkBtm[j].pt3D.z)
{
isPeak = false;
break;
}
}
}
if (true == isPeak)
{
SSG_basicFeature1D a_feature;
a_feature.featureType = LINE_FEATURE_PEAK_BOTTOM;
a_feature.jumpPos = pkBtm[i].pt3D;
a_feature.jumpPos2D = { lineIdx, pkBtm[i].nPointIdx };
localMax.push_back(a_feature);
}
}
return;
}
#if 0
typedef struct
{

264
sourceCode/rodAndBarDetection.cpp
View File

@ -6,12 +6,44 @@
#include <limits>
//version 1.0.0 : base version release to customer
std::string m_strVersion = "1.0.0";
//version 1.1.0 : 添加了地面调平和棒材定位
std::string m_strVersion = "1.1.0";
const char* wd_rodAndBarDetectionVersion(void)
{
return m_strVersion.c_str();
}
//计算一个平面调平参数。
//数据输入中可以有一个地平面和参考调平平面，以最高的平面进行调平
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
SSG_planeCalibPara sx_rodPosition_getBaseCalibPara(
std::vector< std::vector<SVzNL3DPosition>>& scanLines)
{
return sg_getPlaneCalibPara2(scanLines);
}
//相机姿态调平，并去除地面
void sx_rodPosition_lineDataR(
std::vector< SVzNL3DPosition>& a_line,
const double* camPoseR,
double groundH)
{
lineDataRT_vector(a_line, camPoseR, groundH);
}
SVzNL3DPoint _translatePoint(SVzNL3DPoint point, const double rMatrix[9])
{
SVzNL3DPoint result;
double x = point.x * rMatrix[0] + point.y * rMatrix[1] + point.z * rMatrix[2];
double y = point.x * rMatrix[3] + point.y * rMatrix[4] + point.z * rMatrix[5];
double z = point.x * rMatrix[6] + point.y * rMatrix[7] + point.z * rMatrix[8];
result.x = x;
result.y = y;
result.z = z;
return result;
}
SVzNL3DPoint getArcPeak(
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
SWD_segFeature & a_arcFeature,
@ -420,5 +452,235 @@ SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, double matrix3d[9])
}
void rodAarcFeatueDetection(
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
const SSG_cornerParam cornerPara,
const SSG_outlierFilterParam filterParam,
const double rodDiameter,
std::vector<std::vector<SWD_rodArcFeature>>& arcFeatures)
{
int lineNum = (int)scanLines.size();
int linePtNum = (int)scanLines[0].size();
for (int line = 0; line < lineNum; line++)
{
if (line == 424)
int kkk = 1;
std::vector<SVzNL3DPosition>& lineData = scanLines[line];
//滤波，滤除异常点
sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam);
//提取rodArc特征
std::vector<SWD_rodArcFeature> line_rodArcs;
wd_getRodArcFeature_peakCornerMethod(lineData, line, rodDiameter/2, cornerPara, line_rodArcs );
arcFeatures.push_back(line_rodArcs);
}
return;
}
void sx_rodPositioning(
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
const SSG_planeCalibPara poseCalibPara,
const SSG_cornerParam cornerPara,
const SSG_outlierFilterParam filterParam,
const SSG_treeGrowParam growParam,
const SSX_rodParam rodParam,
std::vector<SSX_rodPositionInfo>& rodInfo,
int* errCode)
{
*errCode = 0;
int lineNum = (int)scanLines.size();
if (lineNum == 0)
{
*errCode = SG_ERR_3D_DATA_NULL;
return;
}
int linePtNum = (int)scanLines[0].size();
//判断数据格式是否为grid。算法只能处理grid数据格式
bool isGridData = true;
for (int line = 0; line < lineNum; line++)
{
if (linePtNum != (int)scanLines[line].size())
{
isGridData = false;
break;
}
}
if (false == isGridData)//数据不是网格格式
{
*errCode = SG_ERR_NOT_GRID_FORMAT;
return;
}
for (int i = 0, i_max = (int)scanLines.size(); i < i_max; i++)
{
if (i == 14)
int kkk = 1;
//行处理
//调平，去除地面
double cuttingZ = -1;
sx_rodPosition_lineDataR(scanLines[i], poseCalibPara.planeCalib, cuttingZ);
}
//生成水平扫描数据
std::vector<std::vector<SVzNL3DPosition>> hLines_raw;
hLines_raw.resize(linePtNum);
for (int i = 0; i < linePtNum; i++)
hLines_raw[i].resize(lineNum);
for (int line = 0; line < lineNum; line++)
{
for (int j = 0; j < linePtNum; j++)
{
scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
hLines_raw[j][line] = scanLines[line][j];
hLines_raw[j][line].pt3D.x = scanLines[line][j].pt3D.y;
hLines_raw[j][line].pt3D.y = scanLines[line][j].pt3D.x;
}
}
//在垂直方向上分别提取ARC特征，并进行特征生长
std::vector<std::vector<SWD_rodArcFeature>> arcFeatures_v;
rodAarcFeatueDetection( scanLines, cornerPara, filterParam, rodParam.diameter, arcFeatures_v);
//特征生长
std::vector<SWD_rodArcFeatureTree> rodArcTrees_v;
wd_getRodArcFeatureGrowingTrees(arcFeatures_v, rodArcTrees_v, growParam);
//水平方向
std::vector<std::vector<SWD_rodArcFeature>> arcFeatures_h;
rodAarcFeatueDetection(hLines_raw, cornerPara, filterParam, rodParam.diameter, arcFeatures_h);
//特征生长
std::vector<SWD_rodArcFeatureTree> rodArcTrees_h;
wd_getRodArcFeatureGrowingTrees(arcFeatures_h, rodArcTrees_h, growParam);
if ((rodArcTrees_v.size() == 0) && (rodArcTrees_h.size() == 0))
{
*errCode = SG_ERR_NOT_GRID_FORMAT;
return;
}
int objNum_v = (int)rodArcTrees_v.size();
int objNum_h = (int)rodArcTrees_h.size();
for (int line = 0; line < lineNum; line++)
{
for (int j = 0; j < linePtNum; j++)
{
scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
}
}
//置标志，用于debug
for (int i = 0; i < objNum_v; i++)
{
int nodeNum = (int)rodArcTrees_v[i].treeNodes.size();
for (int j = 0; j < nodeNum; j++)
{
int lineIdx = rodArcTrees_v[i].treeNodes[j].lineIdx;
int centerPtIdx = rodArcTrees_v[i].treeNodes[j].peakPtIdx;
for (int m = rodArcTrees_v[i].treeNodes[j].startPtIdx; m <= rodArcTrees_v[i].treeNodes[j].endPtIdx; m++)
scanLines[lineIdx][m].nPointIdx = 1;
scanLines[lineIdx][centerPtIdx].nPointIdx |= 0x10;
}
}
//置标志，用于debug
for (int i = 0; i < objNum_h; i++)
{
int nodeNum = (int)rodArcTrees_h[i].treeNodes.size();
for (int j = 0; j < nodeNum; j++)
{
int ptIdx = rodArcTrees_h[i].treeNodes[j].lineIdx;
int centerLineIdx = rodArcTrees_h[i].treeNodes[j].peakPtIdx;
for (int m = rodArcTrees_h[i].treeNodes[j].startPtIdx; m <= rodArcTrees_h[i].treeNodes[j].endPtIdx; m++)
scanLines[m][ptIdx].nPointIdx |= 2;
scanLines[centerLineIdx][ptIdx].nPointIdx |= 0x20;
}
}
//目标判断
//(1)长度过滤
for (int i = 0; i < (int)rodArcTrees_v.size(); i++)
{
int nodeSize = rodArcTrees_v[i].treeNodes.size();
SVzNL3DPoint startCenter = rodArcTrees_v[i].treeNodes[0].peakPt;
SVzNL3DPoint endCenter = rodArcTrees_v[i].treeNodes[nodeSize-1].peakPt;
double len = sqrt(pow(startCenter.x - endCenter.x, 2) +
pow(startCenter.y - endCenter.y, 2) +
pow(startCenter.z - endCenter.z, 2));
double lenDiff = abs(len - rodParam.len);
if (lenDiff < 10.0) //validObj
{
//在XY平面内直线拟合
//为了防止端部影响，跳过端面数据
std::vector<SVzNL3DPoint> fittingPoints;
for (int j = 0; j < nodeSize; j++)
{
double dist1 = sqrt(pow(rodArcTrees_v[i].treeNodes[j].peakPt.x - startCenter.x, 2) +
pow(rodArcTrees_v[i].treeNodes[j].peakPt.y - startCenter.y, 2) +
pow(rodArcTrees_v[i].treeNodes[j].peakPt.z - startCenter.z, 2));
double dist2 = sqrt(pow(rodArcTrees_v[i].treeNodes[j].peakPt.x - endCenter.x, 2) +
pow(rodArcTrees_v[i].treeNodes[j].peakPt.y - endCenter.y, 2) +
pow(rodArcTrees_v[i].treeNodes[j].peakPt.z - endCenter.z, 2));
if ((dist1 > rodParam.diameter / 2) && (dist2 > rodParam.diameter / 2))
fittingPoints.push_back(rodArcTrees_v[i].treeNodes[j].peakPt);
}
if (fittingPoints.size() < 3)
continue;
//拟合
double _a, _b, _c;
lineFitting_abc(fittingPoints, &_a, &_b, &_c);
int dataSize = (int)fittingPoints.size();
SVzNL2DPointD foot1 = sx_getFootPoint_abc(fittingPoints[0].x, fittingPoints[0].y, _a, _b, _c);
SVzNL2DPointD foot2 = sx_getFootPoint_abc(fittingPoints[dataSize - 1].x, fittingPoints[dataSize - 1].y, _a, _b, _c);
//直线的轴向向量
SVzNL3DPoint axialDir = { foot2.x - foot1.x, foot2.y - foot1.y, fittingPoints[dataSize - 1].z - fittingPoints[0].z };
double theta = atan2(foot2.y - foot1.y, foot2.x - foot1.x);
double sinTheta = sin(theta);
double cosTheta = cos(theta);
SVzNL3DPoint tmpDir = { sinTheta, -cosTheta, 0 };
SVzNL3DPoint normalDir = { axialDir.y * tmpDir.z - tmpDir.y * axialDir.z,
axialDir.z * tmpDir.x - tmpDir.z * axialDir.x,
axialDir.x * tmpDir.y - tmpDir.x * axialDir.y };
SSX_rodPositionInfo a_objRod;
//确定真正起点和终点：到直线距离小于5mm
double tmpData = sqrt(_a * _a + _b * _b);
_a = _a / tmpData;
_b = _b / tmpData;
_c = _c / tmpData;
SVzNL3DPoint realStart, realEnd;
bool foundStart = false;
for (int j = 0; j < nodeSize; j++)
{
double dist = abs(rodArcTrees_v[i].treeNodes[j].peakPt.x * _a +
rodArcTrees_v[i].treeNodes[j].peakPt.y * _b + _c);
if (dist < 5.0)
{
realStart = rodArcTrees_v[i].treeNodes[j].peakPt;
foundStart = true;
break;
}
}
}
}
//（2）遮挡判断
//将数据重新投射回原来的坐标系，以保持手眼标定结果正确
for (int i = 0; i < lineNum; i++)
sx_rodPosition_lineDataR(scanLines[i], poseCalibPara.invRMatrix, -1);
//将检测结果重新投射回原来的坐标系
for (int i = 0; i < (int)rodInfo.size(); i++)
{
SSX_rodPositionInfo& a_rod = rodInfo[i];
SVzNL3DPoint rawObj = _translatePoint(a_rod.center, poseCalibPara.invRMatrix);
a_rod.center = rawObj;
rawObj = _translatePoint(a_rod.axialDir, poseCalibPara.invRMatrix);
a_rod.axialDir = rawObj;
rawObj = _translatePoint(a_rod.normalDir, poseCalibPara.invRMatrix);
a_rod.normalDir = rawObj;
}
return;
}

39
sourceCode/rodAndBarDetection_Export.h
View File

@ -12,9 +12,38 @@ typedef struct
double rotateAngle; //-30 - 30度
}SSX_hexHeadScrewInfo; //六角头螺杆
typedef struct
{
double diameter;
double len;
}SSX_rodParam; //棒材信息
typedef struct
{
SVzNL3DPoint center; //螺杆端部中心点
SVzNL3DPoint axialDir; //轴向向量
SVzNL3DPoint normalDir; //法向量
SVzNL3DPoint startPt;
SVzNL3DPoint endPt;
}SSX_rodPositionInfo;
//读版本号
SG_APISHARED_EXPORT const char* wd_rodAndBarDetectionVersion(void);
//计算一个平面调平参数。
//数据输入中可以有一个地平面和参考调平平面，以最高的平面进行调平
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
SG_APISHARED_EXPORT SSG_planeCalibPara sx_rodPosition_getBaseCalibPara(
std::vector< std::vector<SVzNL3DPosition>>& scanLines);
#if 0
//相机姿态调平，并去除地面
SG_APISHARED_EXPORT void sx_rodPosition_lineDataR(
std::vector< SVzNL3DPosition>& a_line,
const double* camPoseR,
double groundH);
#endif
//六角头螺杆端部中心点和轴向测量
SG_APISHARED_EXPORT void sx_hexHeadScrewMeasure(
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
@ -25,3 +54,13 @@ SG_APISHARED_EXPORT void sx_hexHeadScrewMeasure(
double rodDiameter,
std::vector<SSX_hexHeadScrewInfo>& screwInfo,
int* errCode);
SG_APISHARED_EXPORT void sx_rodPositioning(
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
const SSG_planeCalibPara poseCalibPara,
const SSG_cornerParam cornerPara,
const SSG_outlierFilterParam filterParam,
const SSG_treeGrowParam growParam,
const SSX_rodParam rodParam,
std::vector<SSX_rodPositionInfo>& rodInfo,
int* errCode);