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

1234 lines
39 KiB
C++
Raw Normal View History

初始提交博清定位纠偏算法 BQ_workpieceCornerExtract
2025-10-22 21:26:30 +08:00
#include <vector>
#include "SG_baseDataType.h"
#include "SG_baseAlgo_Export.h"
#include "BQ_workpieceCornerExtraction_Export.h"
#include <opencv2/opencv.hpp>
#include <limits>
-添加双目Mark相机项目
2025-12-05 18:41:05 +08:00
//version 1.1.0 : base version release to customer, output corner coordinate
//version 1.2.0 : add position length output
std::string m_strVersion = "1.2.0";
BQ_workpieceCornerExtract Version:1.1.0
2025-11-08 02:13:19 +08:00
const char* wd_BQWorkpieceCornerVersion(void)
{
return m_strVersion.c_str();
}
初始提交博清定位纠偏算法 BQ_workpieceCornerExtract
2025-10-22 21:26:30 +08:00
//<2F><><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>п<EFBFBD><D0BF><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD>Ͳο<CDB2><CEBF><EFBFBD>ƽƽ<C6BD>棬<EFBFBD><E6A3AC><EFBFBD><EFBFBD><EFBFBD>ߵ<EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD>е<EFBFBD>ƽ
//<2F><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƽ<EFBFBD>淨<EFBFBD><E6B7A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD>IJ<EFBFBD><C4B2><EFBFBD>
SSG_planeCalibPara sx_BQ_getBaseCalibPara(
std::vector< std::vector<SVzNL3DPosition>>& scanLines)
{
return sg_getPlaneCalibPara2(scanLines);
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̬<EFBFBD><CCAC>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD>ȥ<EFBFBD><C8A5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
void sx_BQ_lineDataR(
std::vector< SVzNL3DPosition>& a_line,
const double* camPoseR,
double groundH)
{
lineDataRT_vector(a_line, camPoseR, groundH);
}
SVzNL3DPoint _translatePoint(SVzNL3DPoint point, 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;
}
//<2F><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ROI
void sg_getTreeROI(SSG_featureTree* a_tree)
{
if (a_tree->treeNodes.size() == 0)
{
a_tree->roi.left = 0;
a_tree->roi.right = 0;
a_tree->roi.top = 0;
a_tree->roi.bottom = 0;
}
else
{
a_tree->roi.left = a_tree->treeNodes[0].jumpPos.x;
a_tree->roi.right = a_tree->treeNodes[0].jumpPos.x;
a_tree->roi.top = a_tree->treeNodes[0].jumpPos.y;
a_tree->roi.bottom = a_tree->treeNodes[0].jumpPos.y;
for (int i = 1, i_max = a_tree->treeNodes.size(); i < i_max; i++)
{
if (a_tree->roi.left > a_tree->treeNodes[i].jumpPos.x)
a_tree->roi.left = a_tree->treeNodes[i].jumpPos.x;
if (a_tree->roi.right < a_tree->treeNodes[i].jumpPos.x)
a_tree->roi.right = a_tree->treeNodes[i].jumpPos.x;
if (a_tree->roi.top > a_tree->treeNodes[i].jumpPos.y)
a_tree->roi.top = a_tree->treeNodes[i].jumpPos.y;
if (a_tree->roi.bottom < a_tree->treeNodes[i].jumpPos.y)
a_tree->roi.bottom = a_tree->treeNodes[i].jumpPos.y;
}
}
return;
}
void _getEdgeContour(SSG_featureTree* a_tree, std::vector<SVzNL3DPoint>& contour, std::vector< std::vector<SVzNL3DPosition>>& scanLines, bool isVScan)
{
for (int j = 0, j_max = (int)a_tree->treeNodes.size(); j < j_max; j++)
{
SSG_basicFeature1D* a_feature = &a_tree->treeNodes[j];
SVzNL3DPoint a_pt;
if (true == isVScan)
a_pt = scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].pt3D;
else
a_pt = scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].pt3D;
if (a_pt.z > 1e-4)//<2F><><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD>˺<EFBFBD><CBBA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0
{
contour.push_back(a_pt);
}
}
}
int _getPointClosestContour(std::vector<SSG_featureTree> trees, bool isVscanTrees, SVzNL3DPoint seedPt, std::vector< std::vector<SVzNL3DPosition>>& scanLines, bool fromHead)
{
double minDist = -1.0;
int idx = -1;
for (int i = 0, i_max = (int)trees.size(); i < i_max; i++)
{
SSG_basicFeature1D a_feature;
if (true == fromHead)
a_feature = trees[i].treeNodes[0];
else
a_feature = trees[i].treeNodes.back();
SVzNL3DPoint a_pt;
if (true == isVscanTrees)
a_pt = scanLines[a_feature.jumpPos2D.x][a_feature.jumpPos2D.y].pt3D;
else
a_pt = scanLines[a_feature.jumpPos2D.y][a_feature.jumpPos2D.x].pt3D;
double dist = sqrt(pow(a_pt.x - seedPt.x, 2) + pow(a_pt.y - seedPt.y, 2));
if (minDist < 0)
{
minDist = dist;
idx = i;
}
else
{
if(dist < minDist)
{
minDist = dist;
idx = i;
}
}
}
return idx;
}
void _getEdgeLinkingContour(SSG_featureTree* a_tree, bool isVScanTree, SVzNL3DPoint seedPt, std::vector<SVzNL3DPoint>& contour, std::vector< std::vector<SVzNL3DPosition>>& scanLines, bool fromHead, double lineLen)
{
for (int i = 0, i_max = (int)a_tree->treeNodes.size(); i < i_max; i++)
{
int idx = i;
if (false == fromHead)
idx = i_max - 1 - i;
SSG_basicFeature1D* a_feature = &a_tree->treeNodes[idx];
SVzNL3DPoint a_pt;
if (true == isVScanTree)
a_pt = scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].pt3D;
else
a_pt = scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].pt3D;
if (a_pt.z > 1e-4)//<2F><><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD>˺<EFBFBD><CBBA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0
{
double dist = sqrt(pow(a_pt.x - seedPt.x, 2) + pow(a_pt.y - seedPt.y, 2));
if (dist > lineLen)
break;
contour.push_back(a_pt);
}
}
}
typedef struct
{
int rgnIdx;
std::vector<SVzNL3DPoint> edge;
SVzNL3DPoint edge_ends[2];
std::vector<SVzNL3DPoint> edgeLink_1;
SVzNL3DPoint edge_link1_ends[2];
std::vector<SVzNL3DPoint> edgeLink_2;
SVzNL3DPoint edge_link2_ends[2];
}SSX_featureContour;
BQ_workpieceCornerExtract Version:1.1.0
2025-11-08 02:13:19 +08:00
typedef struct
{
int lineIdx;
int ptIdx;
double R;
double angle;
double x;
double y;
double z;
}SWD_polarPt;
//<2F><>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD>תʱ <20><> > 0 <20><>˳ʱ<CBB3><CAB1><EFBFBD><EFBFBD>תʱ <20><> < 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;
}
-添加双目Mark相机项目
2025-12-05 18:41:05 +08:00
int _counterLinePtNum(std::vector<SVzNL3DPosition>& lineData)
{
int ptNum = 0;
for (int i = 0, i_max = (int)lineData.size(); i < i_max; i++)
{
if (lineData[i].pt3D.z > 1e-4)
ptNum++;
}
return ptNum;
}
初始提交博清定位纠偏算法 BQ_workpieceCornerExtract
2025-10-22 21:26:30 +08:00
SSX_BQworkpieceResult sx_BQ_getWorkpieceCorners(
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
const SSG_cornerParam cornerPara,
const SSG_outlierFilterParam filterParam,
SSG_treeGrowParam growParam,
SSG_planeCalibPara groundCalibPara,
SSX_BQworkpiecePara workpieceParam,
#if _OUTPUT_DEBUG_DATA
SSX_debugInfo* debug_conturs,
#endif
int* errCode)
{
*errCode = 0;
SSX_BQworkpieceResult workpieceCorners;
memset(&workpieceCorners, 0, sizeof(SSX_BQworkpieceResult));
int lineNum = (int)scanLines.size();
if (lineNum == 0)
{
*errCode = SG_ERR_3D_DATA_NULL;
return workpieceCorners;
}
-添加双目Mark相机项目
2025-12-05 18:41:05 +08:00
//<2F><><EFBFBD><EFBFBD>ʼ<EFBFBD>ͽ<EFBFBD><CDBD><EFBFBD><EFBFBD>Ŀհ<C4BF>ɨ<EFBFBD><C9A8><EFBFBD><EFBFBD>ȥ<EFBFBD><C8A5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɨ<EFBFBD><C9A8><EFBFBD>߽<EFBFBD>
int validStartLine = -1;
for (int i = 0; i < lineNum; i++)
{
int linePtNum = _counterLinePtNum(scanLines[i]);
if (linePtNum > 0)
{
validStartLine = i;
break;
}
}
int validEndLine = -1;
for (int i = lineNum - 1; i >= 0; i--)
{
int linePtNum = _counterLinePtNum(scanLines[i]);
if (linePtNum > 0)
{
validEndLine = i;
break;
}
}
if ( (validStartLine < 0) || (validEndLine < 0))
{
*errCode = SG_ERR_3D_DATA_NULL;
return workpieceCorners;
}
初始提交博清定位纠偏算法 BQ_workpieceCornerExtract
2025-10-22 21:26:30 +08:00
int linePtNum = (int)scanLines[0].size();
bool isGridData = true;
BQ_workpieceCornerExtract Version:1.1.0
2025-11-08 02:13:19 +08:00
SSX_featureContour region[4];
//<2F><><EFBFBD><EFBFBD>Ӧ<EFBFBD><D3A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD>Ƕ<EFBFBD>
初始提交博清定位纠偏算法 BQ_workpieceCornerExtract
2025-10-22 21:26:30 +08:00
{
BQ_workpieceCornerExtract Version:1.1.0
2025-11-08 02:13:19 +08:00
//<2F><>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ
std::vector<std::vector<SSG_basicFeature1D>> jumpFeatures_v_raw;
for (int line = 0; line < lineNum; line++)
{
-添加双目Mark相机项目
2025-12-05 18:41:05 +08:00
if (line == 250)
BQ_workpieceCornerExtract Version:1.1.0
2025-11-08 02:13:19 +08:00
int kkk = 1;
初始提交博清定位纠偏算法 BQ_workpieceCornerExtract
2025-10-22 21:26:30 +08:00
BQ_workpieceCornerExtract Version:1.1.0
2025-11-08 02:13:19 +08:00
std::vector<SVzNL3DPosition>& lineData = scanLines[line];
if (linePtNum != (int)lineData.size())
isGridData = false;
初始提交博清定位纠偏算法 BQ_workpieceCornerExtract
2025-10-22 21:26:30 +08:00
BQ_workpieceCornerExtract Version:1.1.0
2025-11-08 02:13:19 +08:00
//<2F>˲<EFBFBD><CBB2><EFBFBD><EFBFBD>˳<EFBFBD><CBB3>쳣<EFBFBD><ECB3A3>
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ͨ<65><CDA8>ȡbagH<67><48>1/4
line_features);
jumpFeatures_v_raw.push_back(line_features);
}
if (false == isGridData)//<2F><><EFBFBD>ݲ<EFBFBD><DDB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʽ
初始提交博清定位纠偏算法 BQ_workpieceCornerExtract
2025-10-22 21:26:30 +08:00
{
BQ_workpieceCornerExtract Version:1.1.0
2025-11-08 02:13:19 +08:00
*errCode = SG_ERR_NOT_GRID_FORMAT;
return workpieceCorners;
初始提交博清定位纠偏算法 BQ_workpieceCornerExtract
2025-10-22 21:26:30 +08:00
}
BQ_workpieceCornerExtract Version:1.1.0
2025-11-08 02:13:19 +08:00
//<2F><><EFBFBD><EFBFBD>ˮƽɨ<C6BD><C9A8>
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; //<2F><>ԭʼ<D4AD><CABC><EFBFBD>ݵ<EFBFBD><DDB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0<EFBFBD><30><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>ʹ<EFBFBD>ã<EFBFBD>
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<72><63><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ
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];
//<2F>˲<EFBFBD><CBB2><EFBFBD><EFBFBD>˳<EFBFBD><CBB3>쳣<EFBFBD><ECB3A3>
int ptNum = (int)lineData.size();
sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], ptNum, filterParam);
初始提交博清定位纠偏算法 BQ_workpieceCornerExtract
2025-10-22 21:26:30 +08:00
BQ_workpieceCornerExtract Version:1.1.0
2025-11-08 02:13:19 +08:00
std::vector<SSG_basicFeature1D> line_features;
int dataSize = (int)lineData.size();
sg_getLineCornerFeature_BQ(
&hLines_raw[line][0],
dataSize,
line,
groundCalibPara.planeHeight,
cornerPara, //scaleͨ<65><CDA8>ȡbagH<67><48>1/4
line_features);
jumpFeatures_h_raw.push_back(line_features);
}
初始提交博清定位纠偏算法 BQ_workpieceCornerExtract
2025-10-22 21:26:30 +08:00
BQ_workpieceCornerExtract Version:1.1.0
2025-11-08 02:13:19 +08:00
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˳<EFBFBD><CBB3><EFBFBD><EFBFBD><EFBFBD>
//<2F><>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>߷<EFBFBD><DFB7><EFBFBD><EFBFBD><EFBFBD>
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);
}
//ˮƽ<CBAE><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɨ<EFBFBD><C9A8><EFBFBD>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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++)
初始提交博清定位纠偏算法 BQ_workpieceCornerExtract
2025-10-22 21:26:30 +08:00
{
BQ_workpieceCornerExtract Version:1.1.0
2025-11-08 02:13:19 +08:00
int lineIdx = a_lineJumpFeature[pi].jumpPos2D.x;
int ptIdx = a_lineJumpFeature[pi].jumpPos2D.y;
if (scanLines[lineIdx][ptIdx].nPointIdx >= 0)
初始提交博清定位纠偏算法 BQ_workpieceCornerExtract
2025-10-22 21:26:30 +08:00
{
BQ_workpieceCornerExtract Version:1.1.0
2025-11-08 02:13:19 +08:00
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;
初始提交博清定位纠偏算法 BQ_workpieceCornerExtract
2025-10-22 21:26:30 +08:00
}
}
}
BQ_workpieceCornerExtract Version:1.1.0
2025-11-08 02:13:19 +08:00
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++)
初始提交博清定位纠偏算法 BQ_workpieceCornerExtract
2025-10-22 21:26:30 +08:00
{
BQ_workpieceCornerExtract Version:1.1.0
2025-11-08 02:13:19 +08:00
int lineIdx = a_lineJumpFeature[pi].jumpPos2D.y;
int ptIdx = a_lineJumpFeature[pi].jumpPos2D.x;
if (scanLines[lineIdx][ptIdx].nPointIdx >= 0)
初始提交博清定位纠偏算法 BQ_workpieceCornerExtract
2025-10-22 21:26:30 +08:00
{
BQ_workpieceCornerExtract Version:1.1.0
2025-11-08 02:13:19 +08:00
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];
//<2F><>ԭʼ<D4AD><CABC><EFBFBD><EFBFBD><EFBFBD>ϱ<EFBFBD><CFB1>ǣ<EFBFBD>ͬʱ<CDAC><CAB1>Mask<73>ϱ<EFBFBD><CFB1><EFBFBD>
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];
//<2F><>ԭʼ<D4AD><CABC><EFBFBD><EFBFBD><EFBFBD>ϱ<EFBFBD><CFB1>ǣ<EFBFBD>ͬʱ<CDAC><CAB1>Mask<73>ϱ<EFBFBD><CFB1><EFBFBD>
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;
初始提交博清定位纠偏算法 BQ_workpieceCornerExtract
2025-10-22 21:26:30 +08:00
}
}
}
BQ_workpieceCornerExtract Version:1.1.0
2025-11-08 02:13:19 +08:00
#endif
//<2F><><EFBFBD>㼸<EFBFBD><E3BCB8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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;
//<2F><><EFBFBD>㼫<EFBFBD><E3BCAB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>R<EFBFBD><52>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;
}
//<2F><><EFBFBD>Ƕȴ<C7B6>С<EFBFBD><D0A1><EFBFBD><EFBFBD>
std::sort(polarPoints.begin(), polarPoints.end(), compareByAngle);
//<2F><>ȡR<C8A1><52>ֵ<EFBFBD><D6B5>
std::vector<int> rPeaks;
std::vector<SWD_polarPt> polarRPeakPts;
int winSize = contourPtSize / 36; //+-10<31>ȷ<EFBFBD>Χ
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;
//<2F><>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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;
}
初始提交博清定位纠偏算法 BQ_workpieceCornerExtract
2025-10-22 21:26:30 +08:00
BQ_workpieceCornerExtract Version:1.1.0
2025-11-08 02:13:19 +08:00
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;
}
}
//<2F><><EFBFBD><EFBFBD>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
}
初始提交博清定位纠偏算法 BQ_workpieceCornerExtract
2025-10-22 21:26:30 +08:00
BQ_workpieceCornerExtract Version:1.1.0
2025-11-08 02:13:19 +08:00
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 //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
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);
}
初始提交博清定位纠偏算法 BQ_workpieceCornerExtract
2025-10-22 21:26:30 +08:00
BQ_workpieceCornerExtract Version:1.1.0
2025-11-08 02:13:19 +08:00
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());
}
添加vector空检查
2025-11-02 08:57:28 +08:00
}
初始提交博清定位纠偏算法 BQ_workpieceCornerExtract
2025-10-22 21:26:30 +08:00
BQ_workpieceCornerExtract Version:1.1.0
2025-11-08 02:13:19 +08:00
#if 0 //<2F><><EFBFBD>㷨<EFBFBD><E3B7A8><EFBFBD><EFBFBD>Ҫɨ<D2AA><C9A8>Ϊˮƽ-<2D><>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD>
添加错误判断：返回值为-1时
2025-11-02 09:36:53 +08:00
{
BQ_workpieceCornerExtract Version:1.1.0
2025-11-08 02:13:19 +08:00
//<2F><>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ
std::vector<std::vector<SSG_basicFeature1D>> jumpFeatures_v;
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;
//<2F>˲<EFBFBD><CBB2><EFBFBD><EFBFBD>˳<EFBFBD><CBB3>쳣<EFBFBD><ECB3A3>
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ͨ<65><CDA8>ȡbagH<67><48>1/4
line_features);
jumpFeatures_v.push_back(line_features);
}
if (false == isGridData)//<2F><><EFBFBD>ݲ<EFBFBD><DDB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʽ
{
*errCode = SG_ERR_NOT_GRID_FORMAT;
return workpieceCorners;
}
//<2F><><EFBFBD><EFBFBD>ˮƽɨ<C6BD><C9A8>
std::vector<std::vector<SVzNL3DPosition>> hLines;
hLines.resize(linePtNum);
for (int i = 0; i < linePtNum; i++)
hLines[i].resize(lineNum);
for (int line = 0; line < lineNum; line++)
{
for (int j = 0; j < linePtNum; j++)
{
scanLines[line][j].nPointIdx = 0; //<2F><>ԭʼ<D4AD><CABC><EFBFBD>ݵ<EFBFBD><DDB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0<EFBFBD><30><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>ʹ<EFBFBD>ã<EFBFBD>
hLines[j][line] = scanLines[line][j];
hLines[j][line].pt3D.x = scanLines[line][j].pt3D.y;
hLines[j][line].pt3D.y = scanLines[line][j].pt3D.x;
}
}
//ˮƽarc<72><63><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ
std::vector<std::vector<SSG_basicFeature1D>> jumpFeatures_h;
int lineNum_h = (int)hLines.size();
for (int line = 0; line < lineNum_h; line++)
{
if (line == 416)
int kkk = 1;
std::vector<SVzNL3DPosition>& lineData = hLines[line];
//<2F>˲<EFBFBD><CBB2><EFBFBD><EFBFBD>˳<EFBFBD><CBB3>쳣<EFBFBD><ECB3A3>
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[line][0],
dataSize,
line,
groundCalibPara.planeHeight,
cornerPara, //scaleͨ<65><CDA8>ȡbagH<67><48>1/4
line_features);
jumpFeatures_h.push_back(line_features);
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//<2F><>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>߷<EFBFBD><DFB7><EFBFBD><EFBFBD><EFBFBD>
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[line];
if (a_lineJumpFeature.size() > 0)
int kkk = 1;
if (line == 202)
int kkk = 1;
sg_lineFeaturesGrowing(
line,
isLastLine,
a_lineJumpFeature,
v_trees,
growParam);
}
//ˮƽ<CBAE><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɨ<EFBFBD><C9A8><EFBFBD>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
std::vector<SSG_featureTree> h_trees;
for (int line = 0; line < lineNum_h; line++)
{
if (line == 650)
int kkk = 1;
bool isLastLine = false;
if (line == lineNum_h - 1)
isLastLine = true;
std::vector<SSG_basicFeature1D>& a_lineJumpFeature = jumpFeatures_h[line];
sg_lineFeaturesGrowing(
line,
isLastLine,
a_lineJumpFeature,
h_trees,
growParam);
}
//tree<65><65>Ϣ
std::vector<SSG_treeInfo> allTreesInfo; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>߽<EFBFBD>
SSG_treeInfo a_nullTree;
memset(&a_nullTree, 0, sizeof(SSG_treeInfo));
allTreesInfo.push_back(a_nullTree); //<2F><><EFBFBD>ִ洢λ<E6B4A2><CEBB><EFBFBD><EFBFBD>treeIdx<64><78>ͬλ<CDAC>ã<EFBFBD><C3A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//<2F><><EFBFBD>ǣ<EFBFBD><C7A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>б<EFBFBD>ע
int hvTreeIdx = 1;
for (int i = 0, i_max = (int)v_trees.size(); i < i_max; i++)
{
SSG_featureTree* a_vTree = &v_trees[i];
sg_getTreeROI(a_vTree);
//<2F><>¼Tree<65><65><EFBFBD><EFBFBD>Ϣ
SSG_treeInfo a_treeInfo;
a_treeInfo.vTreeFlag = 1;
a_treeInfo.treeIdx = hvTreeIdx;
a_treeInfo.treeType = a_vTree->treeType;
a_treeInfo.sLineIdx = a_vTree->sLineIdx;
a_treeInfo.eLineIdx = a_vTree->eLineIdx;
a_treeInfo.roi = a_vTree->roi;
allTreesInfo.push_back(a_treeInfo);
std::vector<SVzNL3DPoint> a_weld_contour;
//<2F><>ԭʼ<D4AD><CABC><EFBFBD><EFBFBD><EFBFBD>ϱ<EFBFBD><CFB1>ǣ<EFBFBD>ͬʱ<CDAC><CAB1>Mask<73>ϱ<EFBFBD><CFB1><EFBFBD>
for (int j = 0, j_max = (int)a_vTree->treeNodes.size(); j < j_max; j++)
{
SSG_basicFeature1D* a_feature = &a_vTree->treeNodes[j];
if (scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].pt3D.z > 1e-4)//<2F><><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD>˺<EFBFBD><CBBA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0
{
int existEdgeId = scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx >> 16;
if (existEdgeId == 0)
{
scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx = a_feature->featureType;
scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx &= 0xffff;
scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx += hvTreeIdx << 16;
}
a_weld_contour.push_back(scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].pt3D);
}
}
hvTreeIdx++;
}
int hTreeStart = hvTreeIdx;
////<2F><>ע:ˮƽ<CBAE><C6BD><EFBFBD><EFBFBD>
for (int i = 0, i_max = (int)h_trees.size(); i < i_max; i++)
{
SSG_featureTree* a_hTree = &h_trees[i];
sg_getTreeROI(a_hTree);
//<2F><>¼Tree<65><65><EFBFBD><EFBFBD>Ϣ
SSG_treeInfo a_treeInfo;
a_treeInfo.vTreeFlag = 0;
a_treeInfo.treeIdx = hvTreeIdx;
a_treeInfo.treeType = a_hTree->treeType;
a_treeInfo.sLineIdx = a_hTree->sLineIdx;
a_treeInfo.eLineIdx = a_hTree->eLineIdx;
a_treeInfo.roi.left = a_hTree->roi.top; //ˮƽɨ<C6BD><C9A8>xy<78>ǽ<EFBFBD><C7BD><EFBFBD><EFBFBD><EFBFBD>
a_treeInfo.roi.right = a_hTree->roi.bottom;
a_treeInfo.roi.top = a_hTree->roi.left;
a_treeInfo.roi.bottom = a_hTree->roi.right;
allTreesInfo.push_back(a_treeInfo);
std::vector<SVzNL3DPoint> a_weld_contour;
//<2F><>ԭʼ<D4AD><CABC><EFBFBD><EFBFBD><EFBFBD>ϱ<EFBFBD><CFB1>ǣ<EFBFBD>ͬʱ<CDAC><CAB1>Mask<73>ϱ<EFBFBD><CFB1><EFBFBD>
for (int j = 0, j_max = (int)a_hTree->treeNodes.size(); j < j_max; j++)
{
SSG_basicFeature1D* a_feature = &a_hTree->treeNodes[j];
if (scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].pt3D.z > 1e-4)//<2F><><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD>˺<EFBFBD><CBBA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0
{
int existEdgeId = scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx >> 16;
if (existEdgeId == 0)
{
scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx += a_feature->featureType << 4;
scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx &= 0xffff;
scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx += hvTreeIdx << 16;
}
a_weld_contour.push_back(scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].pt3D);
}
}
hvTreeIdx++;
}
int hvTreeSize = hvTreeIdx;
if (v_trees.size() < 2)
{
*errCode = SX_ERR_INVLD_VTREE_NUM;
return workpieceCorners;
}
if (h_trees.size() < 2)
{
*errCode = SX_ERR_INVLD_HTREE_NUM;
return workpieceCorners;
}
//Ѱ<><D1B0>vTree<65><65><EFBFBD><EFBFBD><EFBFBD>Ϻ<EFBFBD><CFBA><EFBFBD><EFBFBD><EFBFBD>
int vTree_T = 0;
int vTree_B = 0;
for (int i = 1, i_max = (int)v_trees.size(); i < i_max; i++)
{
if (v_trees[i].roi.top < v_trees[vTree_T].roi.top)
vTree_T = i;
if (v_trees[i].roi.bottom > v_trees[vTree_B].roi.bottom)
vTree_B = i;
}
//Ѱ<><D1B0>hTree<65><65><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
int hTree_L = 0;
int hTree_R = 0;
for (int i = 1, i_max = (int)h_trees.size(); i < i_max; i++)
{
//ˮƽɨ<C6BD><C9A8>xy<78>ǽ<EFBFBD><C7BD><EFBFBD><EFBFBD>ģ<EFBFBD><C4A3><EFBFBD><EFBFBD>Ҷ<EFBFBD>ӦROI<4F><49>topBottom
if (h_trees[i].roi.top < h_trees[hTree_L].roi.top)
hTree_L = i;
if (h_trees[i].roi.bottom > h_trees[hTree_R].roi.bottom)
hTree_R = i;
}
region[0].rgnIdx = 0; //Left
_getEdgeContour(&h_trees[hTree_L], region[0].edge, scanLines, false);
//Ѱ<>Ҷ<EFBFBD>Ӧ<EFBFBD><D3A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
SVzNL3DPoint firstPt = region[0].edge[0];
SVzNL3DPoint lastPt = region[0].edge.back();
int idx0 = _getPointClosestContour(v_trees, true, firstPt, scanLines, true);
if (idx0 < 0)
{
*errCode = SX_ERR_INVLD_CLOSES_PT;
return workpieceCorners;
}
_getEdgeLinkingContour(&v_trees[idx0], true, firstPt, region[0].edgeLink_1, scanLines, true, workpieceParam.lineLen);
int idx1 = _getPointClosestContour(v_trees, true, lastPt, scanLines, true);
if (idx1 < 0)
{
*errCode = SX_ERR_INVLD_CLOSES_PT;
return workpieceCorners;
}
_getEdgeLinkingContour(&v_trees[idx1], true, lastPt, region[0].edgeLink_2, scanLines, true, workpieceParam.lineLen);
if ((region[0].edgeLink_1.size() < 5) || (region[0].edgeLink_2.size() < 5))
{
*errCode = SX_ERR_INVLD_EDGE_LINK_NUM;
return workpieceCorners;
}
region[1].rgnIdx = 1; //Top
_getEdgeContour(&v_trees[vTree_T], region[1].edge, scanLines, true);
//Ѱ<>Ҷ<EFBFBD>Ӧ<EFBFBD><D3A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
firstPt = region[1].edge[0];
lastPt = region[1].edge.back();
idx0 = _getPointClosestContour(h_trees, false, firstPt, scanLines, true);
if (idx0 < 0)
{
*errCode = SX_ERR_INVLD_CLOSES_PT;
return workpieceCorners;
}
_getEdgeLinkingContour(&h_trees[idx0], false, firstPt, region[1].edgeLink_1, scanLines, true, workpieceParam.lineLen);
idx1 = _getPointClosestContour(h_trees, false, lastPt, scanLines, true);
if (idx1 < 0)
{
*errCode = SX_ERR_INVLD_CLOSES_PT;
return workpieceCorners;
}
_getEdgeLinkingContour(&h_trees[idx1], false, lastPt, region[1].edgeLink_2, scanLines, true, workpieceParam.lineLen);
if ((region[1].edgeLink_1.size() < 5) || (region[1].edgeLink_2.size() < 5))
{
*errCode = SX_ERR_INVLD_EDGE_LINK_NUM;
return workpieceCorners;
}
region[2].rgnIdx = 2; //Right
_getEdgeContour(&h_trees[hTree_R], region[2].edge, scanLines, false);
//Ѱ<>Ҷ<EFBFBD>Ӧ<EFBFBD><D3A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
firstPt = region[2].edge[0];
lastPt = region[2].edge.back();
idx0 = _getPointClosestContour(v_trees, true, firstPt, scanLines, false);
if (idx0 < 0)
{
*errCode = SX_ERR_INVLD_CLOSES_PT;
return workpieceCorners;
}
_getEdgeLinkingContour(&v_trees[idx0], true, firstPt, region[2].edgeLink_1, scanLines, false, workpieceParam.lineLen);
idx1 = _getPointClosestContour(v_trees, true, lastPt, scanLines, false);
if (idx1 < 0)
{
*errCode = SX_ERR_INVLD_CLOSES_PT;
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);
//Ѱ<>Ҷ<EFBFBD>Ӧ<EFBFBD><D3A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
firstPt = region[3].edge[0];
lastPt = region[3].edge.back();
idx0 = _getPointClosestContour(h_trees, false, firstPt, scanLines, true);
if (idx0 < 0)
{
*errCode = SX_ERR_INVLD_CLOSES_PT;
return workpieceCorners;
}
_getEdgeLinkingContour(&h_trees[idx0], false, firstPt, region[3].edgeLink_1, scanLines, false, workpieceParam.lineLen);
idx1 = _getPointClosestContour(h_trees, false, lastPt, scanLines, true);
if (idx1 < 0)
{
*errCode = SX_ERR_INVLD_CLOSES_PT;
return workpieceCorners;
}
_getEdgeLinkingContour(&h_trees[idx1], false, lastPt, region[3].edgeLink_2, scanLines, false, workpieceParam.lineLen);
if ((region[3].edgeLink_1.size() < 5) || (region[3].edgeLink_2.size() < 5))
{
*errCode = SX_ERR_INVLD_EDGE_LINK_NUM;
return workpieceCorners;
}
添加vector空检查
2025-11-02 08:57:28 +08:00
}
BQ_workpieceCornerExtract Version:1.1.0
2025-11-08 02:13:19 +08:00
#endif
初始提交博清定位纠偏算法 BQ_workpieceCornerExtract
2025-10-22 21:26:30 +08:00
for (int i = 0; i < 4; i++)
{
if ((i == 0) || (i == 2))
{
//Left:<3A><>ֹ<EFBFBD><D6B9>ֱֱ<D6B1>ߣ<EFBFBD>ʹ<EFBFBD><CAB9>x=ky+b
std::vector<SVzNL3DPoint> transPts;
for (int m = 0, m_max = (int)region[i].edge.size(); m < m_max; m++)
{
SVzNL3DPoint a_pt;
a_pt.x = region[i].edge[m].y;
a_pt.y = region[i].edge[m].x;
a_pt.z = region[i].edge[m].z;
transPts.push_back(a_pt);
}
//<2F><><EFBFBD>ϲ<EFBFBD><CFB2><EFBFBD>
double edge_x_k, edge_x_b;
lineFitting(transPts, &edge_x_k, &edge_x_b);
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֱ<EFBFBD>߶˵<DFB6>
SVzNL3DPoint end_0 = transPts[0];
SVzNL3DPoint end_1 = transPts.back();
SVzNL2DPointD foot_0 = sx_getFootPoint(end_0.x, end_0.y, edge_x_k, edge_x_b);
SVzNL2DPointD foot_1 = sx_getFootPoint(end_1.x, end_1.y, edge_x_k, edge_x_b);
region[i].edge_ends[0] = { foot_0.y, foot_0.x, groundCalibPara.planeHeight };
region[i].edge_ends[1] = { foot_1.y, foot_1.x, groundCalibPara.planeHeight };
//<2F><><EFBFBD><EFBFBD>Linkingֱ<67><D6B1>ʹ<EFBFBD><CAB9> y=kx+b
double edge_link1_k, edge_link1_b;
lineFitting(region[i].edgeLink_1, &edge_link1_k, &edge_link1_b);
end_0 = region[i].edgeLink_1[0];
end_1 = region[i].edgeLink_1.back();
foot_0 = sx_getFootPoint(end_0.x, end_0.y, edge_link1_k, edge_link1_b);
foot_1 = sx_getFootPoint(end_1.x, end_1.y, edge_link1_k, edge_link1_b);
region[i].edge_link1_ends[0] = { foot_0.x, foot_0.y, groundCalibPara.planeHeight };
region[i].edge_link1_ends[1] = { foot_1.x, foot_1.y, groundCalibPara.planeHeight };
double edge_link2_k, edge_link2_b;
lineFitting(region[i].edgeLink_2, &edge_link2_k, &edge_link2_b);
end_0 = region[i].edgeLink_2[0];
end_1 = region[i].edgeLink_2.back();
foot_0 = sx_getFootPoint(end_0.x, end_0.y, edge_link2_k, edge_link2_b);
foot_1 = sx_getFootPoint(end_1.x, end_1.y, edge_link2_k, edge_link2_b);
region[i].edge_link2_ends[0] = { foot_0.x, foot_0.y, groundCalibPara.planeHeight };
region[i].edge_link2_ends[1] = { foot_1.x, foot_1.y, groundCalibPara.planeHeight };
//<2F><><EFBFBD>㽻<EFBFBD><E3BDBB>
end_0 = region[i].edge[0];
end_1 = region[i].edge.back();
SVzNL3DPoint crossPt[3];
crossPt[0].x = (edge_x_k * edge_link1_b + edge_x_b) / (1.0 - edge_x_k * edge_link1_k);
crossPt[0].y = edge_link1_k * crossPt[0].x + edge_link1_b;
crossPt[0].z = groundCalibPara.planeHeight;
crossPt[2].x = (edge_x_k * edge_link2_b + edge_x_b) / (1.0 - edge_x_k * edge_link2_k);
crossPt[2].y = edge_link2_k * crossPt[2].x + edge_link2_b;
crossPt[2].z = groundCalibPara.planeHeight;
crossPt[1].x = (crossPt[0].x + crossPt[2].x) / 2;
crossPt[1].y = (crossPt[0].y + crossPt[2].y) / 2;
crossPt[1].z = groundCalibPara.planeHeight;
if (i == 0)
{
for (int m = 0; m < 3; m++)
workpieceCorners.corner_L[m] = crossPt[m];
}
else
{
for (int m = 0; m < 3; m++)
workpieceCorners.corner_R[m] = crossPt[m];
}
}
else
{
//<2F><><EFBFBD>ϲ<EFBFBD><CFB2><EFBFBD>
double edge_k, edge_b;
lineFitting(region[i].edge, &edge_k, &edge_b);
SVzNL3DPoint end_0 = region[i].edge[0];
SVzNL3DPoint end_1 = region[i].edge.back();
SVzNL2DPointD foot_0 = sx_getFootPoint(end_0.x, end_0.y, edge_k, edge_b);
SVzNL2DPointD foot_1 = sx_getFootPoint(end_1.x, end_1.y, edge_k, edge_b);
region[i].edge_ends[0] = { foot_0.x, foot_0.y, groundCalibPara.planeHeight };
region[i].edge_ends[1] = { foot_1.x, foot_1.y, groundCalibPara.planeHeight };
//<2F><>ֹ<EFBFBD><D6B9>ֱֱ<D6B1>ߣ<EFBFBD>ʹ<EFBFBD><CAB9>x=ky+b
std::vector<SVzNL3DPoint> transPts_link1;
for (int m = 0, m_max = (int)region[i].edgeLink_1.size(); m < m_max; m++)
{
SVzNL3DPoint a_pt;
a_pt.x = region[i].edgeLink_1[m].y;
a_pt.y = region[i].edgeLink_1[m].x;
a_pt.z = region[i].edgeLink_1[m].z;
transPts_link1.push_back(a_pt);
}
double edge_link1_kx, edge_link1_bx;
lineFitting(transPts_link1, &edge_link1_kx, &edge_link1_bx);
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֱ<EFBFBD>߶˵<DFB6>
end_0 = transPts_link1[0];
end_1 = transPts_link1.back();
foot_0 = sx_getFootPoint(end_0.x, end_0.y, edge_link1_kx, edge_link1_bx);
foot_1 = sx_getFootPoint(end_1.x, end_1.y, edge_link1_kx, edge_link1_bx);
region[i].edge_link1_ends[0] = { foot_0.y, foot_0.x, groundCalibPara.planeHeight };
region[i].edge_link1_ends[1] = { foot_1.y, foot_1.x, groundCalibPara.planeHeight };
//<2F><><EFBFBD><EFBFBD>Linkingֱ<67><D6B1>ʹ<EFBFBD><CAB9> y=kx+b
std::vector<SVzNL3DPoint> transPts_link2;
for (int m = 0, m_max = (int)region[i].edgeLink_2.size(); m < m_max; m++)
{
SVzNL3DPoint a_pt;
a_pt.x = region[i].edgeLink_2[m].y;
a_pt.y = region[i].edgeLink_2[m].x;
a_pt.z = region[i].edgeLink_2[m].z;
transPts_link2.push_back(a_pt);
}
double edge_link2_kx, edge_link2_bx;
lineFitting(transPts_link2, &edge_link2_kx, &edge_link2_bx);
end_0 = transPts_link2[0];
end_1 = transPts_link2.back();
foot_0 = sx_getFootPoint(end_0.x, end_0.y, edge_link2_kx, edge_link2_bx);
foot_1 = sx_getFootPoint(end_1.x, end_1.y, edge_link2_kx, edge_link2_bx);
region[i].edge_link2_ends[0] = { foot_0.y, foot_0.x, groundCalibPara.planeHeight };
region[i].edge_link2_ends[1] = { foot_1.y, foot_1.x, groundCalibPara.planeHeight };
BQ_workpieceCornerExtract Version:1.1.0
2025-11-08 02:13:19 +08:00
初始提交博清定位纠偏算法 BQ_workpieceCornerExtract
2025-10-22 21:26:30 +08:00
//<2F><><EFBFBD>㽻<EFBFBD><E3BDBB>
end_0 = region[i].edge[0];
end_1 = region[i].edge.back();
SVzNL3DPoint crossPt[3];
crossPt[0].x = (edge_link1_kx * edge_b + edge_link1_bx) / (1.0 - edge_link1_kx * edge_k);
crossPt[0].y = edge_k * crossPt[0].x + edge_b;
crossPt[0].z = groundCalibPara.planeHeight;
crossPt[2].x = (edge_link2_kx * edge_b + edge_link2_bx) / (1.0 - edge_link2_kx * edge_k);
crossPt[2].y = edge_k * crossPt[2].x + edge_b;
crossPt[2].z = groundCalibPara.planeHeight;
crossPt[1].x = (crossPt[0].x + crossPt[2].x) / 2;
crossPt[1].y = (crossPt[0].y + crossPt[2].y) / 2;
crossPt[1].z = groundCalibPara.planeHeight;
if (i == 1)
{
for (int m = 0; m < 3; m++)
workpieceCorners.corner_T[m] = crossPt[m];
}
else
{
for (int m = 0; m < 3; m++)
workpieceCorners.corner_B[m] = crossPt[m];
}
}
}
BQ_workpieceCornerExtract Version:1.1.0
2025-11-08 02:13:19 +08:00
初始提交博清定位纠偏算法 BQ_workpieceCornerExtract
2025-10-22 21:26:30 +08:00
#if 1
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ͷ<EFBFBD><CDB6><EFBFBD><EFBFBD>ԭ<EFBFBD><D4AD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD>Ա<EFBFBD><D4B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>۱궨<DBB1><EAB6A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȷ
for (int i = 0; i < lineNum; i++)
sx_BQ_lineDataR(scanLines[i], groundCalibPara.invRMatrix, -1);
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ͷ<EFBFBD><CDB6><EFBFBD><EFBFBD>ԭ<EFBFBD><D4AD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ
SVzNL3DPoint rawObj;
for (int i = 0; i < 3; i++)
{
rawObj = _translatePoint(workpieceCorners.corner_L[i], groundCalibPara.invRMatrix);
workpieceCorners.corner_L[i] = rawObj;
rawObj = _translatePoint(workpieceCorners.corner_R[i], groundCalibPara.invRMatrix);
workpieceCorners.corner_R[i] = rawObj;
rawObj = _translatePoint(workpieceCorners.corner_T[i], groundCalibPara.invRMatrix);
workpieceCorners.corner_T[i] = rawObj;
rawObj = _translatePoint(workpieceCorners.corner_B[i], groundCalibPara.invRMatrix);
workpieceCorners.corner_B[i] = rawObj;
}
#if _OUTPUT_DEBUG_DATA
for (int i = 0; i < 4; i++)
{
rawObj = _translatePoint(region[i].edge_ends[0], groundCalibPara.invRMatrix);
region[i].edge_ends[0] = rawObj;
rawObj = _translatePoint(region[i].edge_ends[1], groundCalibPara.invRMatrix);
region[i].edge_ends[1] = rawObj;
rawObj = _translatePoint(region[i].edge_link1_ends[0], groundCalibPara.invRMatrix);
region[i].edge_link1_ends[0] = rawObj;
rawObj = _translatePoint(region[i].edge_link1_ends[1], groundCalibPara.invRMatrix);
region[i].edge_link1_ends[1] = rawObj;
rawObj = _translatePoint(region[i].edge_link2_ends[0], groundCalibPara.invRMatrix);
region[i].edge_link2_ends[0] = rawObj;
rawObj = _translatePoint(region[i].edge_link2_ends[1], groundCalibPara.invRMatrix);
region[i].edge_link2_ends[1] = rawObj;
for (int m = 0, m_max = (int)region[i].edge.size(); m < m_max; m++)
{
rawObj = _translatePoint(region[i].edge[m], groundCalibPara.invRMatrix);
region[i].edge[m] = rawObj;
}
for (int m = 0, m_max = (int)region[i].edgeLink_1.size(); m < m_max; m++)
{
rawObj = _translatePoint(region[i].edgeLink_1[m], groundCalibPara.invRMatrix);
region[i].edgeLink_1[m] = rawObj;;
}
for (int m = 0, m_max = (int)region[i].edgeLink_2.size(); m < m_max; m++)
{
rawObj = _translatePoint(region[i].edgeLink_2[m], groundCalibPara.invRMatrix);
region[i].edgeLink_2[m] = rawObj;;
}
}
#endif
#endif
#if _OUTPUT_DEBUG_DATA
if (debug_conturs)
{
for (int i = 0; i < 4; i++)
{
debug_conturs[i].rgnIdx = region[i].rgnIdx;
debug_conturs[i].edge_ends[0] = region[i].edge_ends[0];
debug_conturs[i].edge_ends[1] = region[i].edge_ends[1];
debug_conturs[i].edge_link1_ends[0] = region[i].edge_link1_ends[0];
debug_conturs[i].edge_link1_ends[1] = region[i].edge_link1_ends[1];
debug_conturs[i].edge_link2_ends[0] = region[i].edge_link2_ends[0];
debug_conturs[i].edge_link2_ends[1] = region[i].edge_link2_ends[1];
debug_conturs[i].edge_size = (int)region[i].edge.size();
debug_conturs[i].edge = (SVzNL3DPoint*)malloc(sizeof(SVzNL3DPoint) * (int)region[i].edge.size());
for (int m = 0, m_max = (int)region[i].edge.size(); m < m_max; m++)
debug_conturs[i].edge[m] = region[i].edge[m];
debug_conturs[i].edgeLink1_size = (int)region[i].edgeLink_1.size();
debug_conturs[i].edgeLink_1 = (SVzNL3DPoint*)malloc(sizeof(SVzNL3DPoint) * (int)region[i].edgeLink_1.size());
for (int m = 0, m_max = (int)region[i].edgeLink_1.size(); m < m_max; m++)
debug_conturs[i].edgeLink_1[m] = region[i].edgeLink_1[m];
debug_conturs[i].edgeLink2_size = (int)region[i].edgeLink_2.size();
debug_conturs[i].edgeLink_2 = (SVzNL3DPoint*)malloc(sizeof(SVzNL3DPoint) * (int)region[i].edgeLink_2.size());
for (int m = 0, m_max = (int)region[i].edgeLink_2.size(); m < m_max; m++)
debug_conturs[i].edgeLink_2[m] = region[i].edgeLink_2[m];
}
}
#endif
workpieceCorners.workpieceType = 1;
return workpieceCorners;
}
Reference in New Issue Copy Permalink