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

580 lines
18 KiB
C++
Raw Normal View History

workpieceHolePositioning v1.0.0 初始版本提交
2026-01-26 15:57:19 +08:00
#include <vector>
#include "SG_baseDataType.h"
#include "SG_baseAlgo_Export.h"
#include "workpieceHolePositioning_Export.h"
#include <opencv2/opencv.hpp>
#include <limits>
//version 1.0.0 : base version release to customer
workpieceHolePositioning v1.0.1 添加了工件姿态（欧拉角）输出
2026-01-30 00:10:14 +08:00
//version 1.0.2 : <20><><EFBFBD><EFBFBD><EFBFBD>˹<EFBFBD><CBB9><EFBFBD><EFBFBD><EFBFBD>̬<EFBFBD><CCAC>ŷ<EFBFBD><C5B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>)
workpieceHolePositioning v1.1.0 对工件姿态规范化为中心点（操作点）加三个方向矢量
2026-01-31 14:36:54 +08:00
//version 1.1.0 : c<>Թ<EFBFBD><D4B9><EFBFBD><EFBFBD><EFBFBD>̬<EFBFBD>淶<EFBFBD><E6B7B6>Ϊ<EFBFBD><CEAA><EFBFBD>ĵ㣨<C4B5><E3A3A8><EFBFBD><EFBFBD><EFBFBD>㣩<EFBFBD><E3A3A9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʸ<EFBFBD><CAB8>
std::string m_strVersion = "1.1.0";
workpieceHolePositioning v1.0.0 初始版本提交
2026-01-26 15:57:19 +08:00
const char* wd_workpieceHolePositioningVersion(void)
{
return m_strVersion.c_str();
}
//<2F><><EFBFBD><EFBFBD>ˮƽ<CBAE><C6BD>װ<EFBFBD><D7B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//<2F><><EFBFBD><EFBFBD>Z<EFBFBD><5A><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƽ<EFBFBD>е<EFBFBD><D0B5><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD>Ҫ<EFBFBD>Ե<EFBFBD><D4B5><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD>գ<EFBFBD><D5A3><EFBFBD><EFBFBD><EFBFBD><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 wd_getGroundCalibPara(
std::vector< std::vector<SVzNL3DPosition>>& scanLines)
{
return sg_getPlaneCalibPara2(scanLines);
}
//<2F><><EFBFBD><EFBFBD>ˮƽʱ<C6BD><CAB1>̬<EFBFBD><CCAC>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD>ȥ<EFBFBD><C8A5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
void wd_lineDataR(
std::vector< SVzNL3DPosition>& a_line,
const double* camPoseR,
double groundH)
{
lineDataRT_vector(a_line, camPoseR, groundH);
}
SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, const double matrix3d[9])
{
SVzNL3DPoint _r_pt;
_r_pt.x = pt3D.x * matrix3d[0] + pt3D.y * matrix3d[1] + pt3D.z * matrix3d[2];
_r_pt.y = pt3D.x * matrix3d[3] + pt3D.y * matrix3d[4] + pt3D.z * matrix3d[5];
_r_pt.z = pt3D.x * matrix3d[6] + pt3D.y * matrix3d[7] + pt3D.z * matrix3d[8];
return _r_pt;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ӽ<EFBFBD>distance<63><65>Ŀ<EFBFBD><C4BF>
int distanceSearchObject(SVzNL3DPoint seed, std::vector<SWD_HoleInfo>& holes, double distance, double distDeviation)
{
int result = -1;
int holeSize = (int)holes.size();
double minDistDiff = DBL_MAX;
int minDistIndex = -1;
for (int i = 0; i < holeSize; i++)
{
if (holes[i].radius < 0)
continue;
double dist = sqrt(pow(seed.x - holes[i].center.x, 2) + pow(seed.y - holes[i].center.y, 2));
double distDiff = abs(dist - distance);
if (minDistDiff > distDiff)
{
minDistDiff = distDiff;
minDistIndex = i;
}
}
if ((minDistIndex >= 0) && (minDistDiff < distDeviation))
result = minDistIndex;
return result;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ӽ<EFBFBD>distance<63>ҽǶ<D2BD>Ϊangle<6C><65>Ŀ<EFBFBD><C4BF>, <20>ԽǶ<D4BD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD>
int angleConditionDistanceSearch(
SVzNL3DPoint seed, SVzNL3DPoint angleSide,
std::vector<SWD_HoleInfo>& holes,
double distance, double distDeviation,
SVzNLRangeD angleRange)
{
int result = -1;
int holeSize = (int)holes.size();
std::vector< int> distValidHoleIndex;
for (int i = 0; i < holeSize; i++)
{
if (holes[i].radius < 0)
continue;
double dist = sqrt(pow(seed.x - holes[i].center.x, 2) + pow(seed.y - holes[i].center.y, 2));
double distDiff = abs(dist - distance);
if (distDiff < distDeviation)
{
distValidHoleIndex.push_back(i);
}
}
if (distValidHoleIndex.size() == 1)
{
int idx = distValidHoleIndex[0];
double angle = computeXOYVertexAngle(seed, angleSide, holes[idx].center);
if( (angle >= angleRange.min) &&(angle <= angleRange.max))
result = idx;
}
else if (distValidHoleIndex.size() > 1)
{
double bestAngle = (angleRange.min + angleRange.max) / 2;
double minAngleDeviateion = DBL_MAX;
int minAngleIdx = -1;
for (int i = 0, i_max = (int)distValidHoleIndex.size(); i < i_max; i++)
{
int idx = distValidHoleIndex[i];
double angle = computeXOYVertexAngle(seed, angleSide, holes[idx].center);
if ((angle >= angleRange.min) && (angle <= angleRange.max))
{
double angleDiff = abs(angle - bestAngle);
if (minAngleDeviateion > angleDiff)
{
minAngleDeviateion = angleDiff;
minAngleIdx = idx;
}
}
}
result = minAngleIdx;
}
return result;
}
workpieceHolePositioning v1.0.1 初始发布版本。
2026-01-26 22:36:13 +08:00
double _getMeanZ(std::vector<std::vector<double>>& quantiValue, SVzNL3DPoint seed, SVzNLRect& roi2D, double rectR)
{
int cols = (int)quantiValue.size();
int rows = (int)quantiValue[0].size();
int px = (int)seed.x - roi2D.left;
int py = (int)seed.y - roi2D.top;
int win = (int)rectR;
int hist = 0;
double zSum = 0;
for (int i = -win; i <= win; i++)
{
for (int j = -win; j <= win; j++)
{
int qx = px + i;
int qy = py + j;
if ((qx >= 0) && (qx < cols) && (qy >= 0) && (qy < rows))
{
if (quantiValue[qx][qy] > 1e-4)
{
zSum += quantiValue[qx][qy];
hist++;
}
}
}
}
if (hist == 0)
return 0;
else
return (zSum / hist);
}
workpieceHolePositioning v1.0.0 初始版本提交
2026-01-26 15:57:19 +08:00
//<2F><><EFBFBD><EFBFBD><EFBFBD>׶<EFBFBD>λ
void wd_workpieceHolePositioning(
std::vector< std::vector<SVzNL3DPosition>>& scanLinesInput,
const WD_workpieceHoleParam workpiecePara,
const SSG_lineSegParam lineSegPara,
const SSG_outlierFilterParam filterParam,
const SSG_treeGrowParam growParam,
const SSG_planeCalibPara groundCalibPara,
std::vector< WD_workpieceInfo>& workpiecePositioning,
int* errCode)
{
*errCode = 0;
int lineNum = (int)scanLinesInput.size();
std::vector< std::vector<SVzNL3DPosition>> scanLines;
scanLines.resize(lineNum);
int linePtNum = (int)scanLinesInput[0].size();
bool isGridData = true;
for (int i = 0; i < lineNum; i++)
{
if (linePtNum != (int)scanLinesInput[i].size())
isGridData = false;
scanLines[i].resize(scanLinesInput[i].size());
std::copy(scanLinesInput[i].begin(), scanLinesInput[i].end(), scanLines[i].begin()); // ʹ<><CAB9>std::copy<70>㷨
}
if (false == isGridData)//<2F><><EFBFBD>ݲ<EFBFBD><DDB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʽ
{
*errCode = SG_ERR_NOT_GRID_FORMAT;
return;
}
for (int i = 0; i < lineNum; i++)
{ //<2F>д<EFBFBD><D0B4><EFBFBD>
//<2F><>ƽ<EFBFBD><C6BD>ȥ<EFBFBD><C8A5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
wd_lineDataR(scanLines[i], groundCalibPara.planeCalib, -1);
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݣ<EFBFBD><DDA3><EFBFBD>1mmΪ<6D><CEAA><EFBFBD><EFBFBD><EFBFBD>߶ȣ<DFB6><C8A3><EFBFBD><EFBFBD><EFBFBD>ȷ<EFBFBD><C8B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>߶<EFBFBD>
workpieceHolePositioning v1.0.1 初始发布版本。
2026-01-26 22:36:13 +08:00
SVzNL3DRangeD roi3D = sg_getScanDataROI_vector( scanLines);
SVzNLRect roi2D;
roi2D.left = (int)roi3D.xRange.min;
roi2D.right = (int)roi3D.xRange.max;
roi2D.top = (int)roi3D.yRange.min;
roi2D.bottom = (int)roi3D.yRange.max;
int quanti_X = roi2D.right - roi2D.left + 1;
int quanti_Y = roi2D.bottom - roi2D.top + 1;
std::vector<std::vector<double>> quantiValue;
std::vector<std::vector<int>> quantiHist;
quantiValue.resize(quanti_X);
quantiHist.resize(quanti_X);
for (int i = 0; i < quanti_X; i++)
{
quantiValue[i].resize(quanti_Y);
std::fill(quantiValue[i].begin(), quantiValue[i].end(), 0);//<2F><>ʼ<EFBFBD><CABC>Ϊ0
quantiHist[i].resize(quanti_Y);
std::fill(quantiHist[i].begin(), quantiHist[i].end(), 0);//<2F><>ʼ<EFBFBD><CABC>Ϊ0
}
//<2F><>1mm<6D>߶<EFBFBD><DFB6><EFBFBD><EFBFBD><EFBFBD>
for (int line = 0; line < lineNum; line++)
{
for (int j = 0; j < linePtNum; j++)
{
SVzNL3DPoint& a_pt = scanLines[line][j].pt3D;
if (a_pt.z > 1e-4)
{
int qx = (int)a_pt.x - roi2D.left;
int qy = (int)a_pt.y - roi2D.top;
quantiValue[qx][qy] += a_pt.z;
quantiHist[qx][qy] += 1;
}
}
}
for (int ix = 0; ix < quanti_X; ix++)
{
for (int iy = 0; iy < quanti_Y; iy++)
{
if (quantiHist[ix][iy] > 0)
quantiValue[ix][iy] = quantiValue[ix][iy] / quantiHist[ix][iy];
}
}
workpieceHolePositioning v1.0.0 初始版本提交
2026-01-26 15:57:19 +08:00
std::vector<std::vector<int>> pointMask;
pointMask.resize(lineNum);
std::vector<SVzNL3DPoint> endingPoints;
//<2F><>ȡ<EFBFBD>߶ζ˵<CEB6><CBB5><EFBFBD><EFBFBD><EFBFBD>
for (int line = 0; line < lineNum; line++)
{
if (line == 1677)
int kkk = 1;
std::vector<SVzNL3DPosition>& lineData = scanLines[line];
pointMask[line].resize(lineData.size());
std::fill(pointMask[line].begin(), pointMask[line].end(), 0);//<2F><>ʼ<EFBFBD><CABC>Ϊ0
//<2F>˲<EFBFBD><CBB2><EFBFBD><EFBFBD>˳<EFBFBD><CBB3>쳣<EFBFBD><ECB3A3>
sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam);
std::vector<SSG_RUN> segs;
wd_getLineDataIntervals(
lineData,
lineSegPara,
segs);
//<2F><>seg<65>˵<EFBFBD><CBB5><EFBFBD>Ϊ<EFBFBD><CEAA>Ե<EFBFBD>㡣<EFBFBD><E3A1A3><EFBFBD>˵<EFBFBD><CBB5><EFBFBD><EFBFBD><EFBFBD>ƽ<EFBFBD>󣬴<EFBFBD>ֱ<EFBFBD>׵<EFBFBD><D7B5>ڲ<EFBFBD><DAB2><EFBFBD>XYƽ<59><C6BD><EFBFBD>Ͼ<EFBFBD>Ϊ<EFBFBD><CEAA>Ե<EFBFBD>㡣
for (int i = 0, i_max = (int)segs.size(); i < i_max; i++)
{
int ptIdx = segs[i].start;
endingPoints.push_back(lineData[ptIdx].pt3D);
pointMask[line][ptIdx] = 1; //<2F><>ֹ<EFBFBD>ظ<EFBFBD>
ptIdx = segs[i].start + segs[i].len - 1;
endingPoints.push_back(lineData[ptIdx].pt3D);
pointMask[line][ptIdx] = 1;
}
}
//<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>ȡ
int lineNum_h_raw = (int)hLines_raw.size();
for (int line = 0; line < lineNum_h_raw; line++)
{
if (line == 974)
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);
std::vector<SSG_RUN> segs;
wd_getLineDataIntervals(
lineData,
lineSegPara,
segs);
//<2F><>seg<65>˵<EFBFBD><CBB5><EFBFBD>Ϊ<EFBFBD><CEAA>Ե<EFBFBD>㡣<EFBFBD><E3A1A3><EFBFBD>˵<EFBFBD><CBB5><EFBFBD><EFBFBD><EFBFBD>ƽ<EFBFBD>󣬴<EFBFBD>ֱ<EFBFBD>׵<EFBFBD><D7B5>ڲ<EFBFBD><DAB2><EFBFBD>XYƽ<59><C6BD><EFBFBD>Ͼ<EFBFBD>Ϊ<EFBFBD><CEAA>Ե<EFBFBD>㡣
for (int i = 0, i_max = (int)segs.size(); i < i_max; i++)
{
int ptIdx = segs[i].start;
if (pointMask[ptIdx][line] == 0) //<2F><>ֹ<EFBFBD><D6B9><EFBFBD>ظ<EFBFBD>
{
SVzNL3DPoint an_ending;
an_ending.x = lineData[ptIdx].pt3D.y;
an_ending.y = lineData[ptIdx].pt3D.x;
an_ending.z = lineData[ptIdx].pt3D.z;
endingPoints.push_back(an_ending);
pointMask[ptIdx][line] = 1;
}
ptIdx = segs[i].start + segs[i].len - 1;
if (pointMask[ptIdx][line] == 0) //<2F><>ֹ<EFBFBD><D6B9><EFBFBD>ظ<EFBFBD>
{
SVzNL3DPoint an_ending;
an_ending.x = lineData[ptIdx].pt3D.y;
an_ending.y = lineData[ptIdx].pt3D.x;
an_ending.z = lineData[ptIdx].pt3D.z;
endingPoints.push_back(an_ending);
pointMask[ptIdx][line] = 1;
}
}
}
//<2F><>ע
std::vector<std::vector<SSG_featureClusteringInfo>> featureInfoMask;
std::vector<std::vector<SVzNL3DPoint>> feature3DInfo;
featureInfoMask.resize(lineNum);
feature3DInfo.resize(lineNum);
for (int i = 0; i < lineNum; i++)
{
featureInfoMask[i].resize(lineNum_h_raw);
feature3DInfo[i].resize(lineNum_h_raw);
}
//<2F><>ע
for (int line = 0; line < lineNum; line++)
{
std::vector<int>& a_lineMask = pointMask[line];
for (int m = 0; m < lineNum_h_raw; m++)
{
if (a_lineMask[m] > 0)
{
SSG_featureClusteringInfo& a_featureInfo = featureInfoMask[line][m];
a_featureInfo.clusterID = 0;
a_featureInfo.featurType = 1;
a_featureInfo.featureIdx_v = 0;
a_featureInfo.featureIdx_h = 0;
a_featureInfo.lineIdx = line;
a_featureInfo.ptIdx = m;
a_featureInfo.flag = 0;
feature3DInfo[line][m] = scanLines[line][m].pt3D;
}
}
}
//<2F><><EFBFBD><EFBFBD>
//<2F><><EFBFBD>õ<EFBFBD><C3B5><EFBFBD>˼<EFBFBD>룬<EFBFBD>ع<EFBFBD>˼·<CBBC><C2B7><EFBFBD>и<EFBFBD>Ч<EFBFBD><D0A7><EFBFBD><EFBFBD>
std::vector<std::vector< SVzNL2DPoint>> clusters; //ֻ<><D6BB>¼λ<C2BC><CEBB>
std::vector<SVzNL3DRangeD> clustersRoi3D;
int clusterID = 1;
int clusterCheckWin = 5;
for (int y = 0; y < lineNum_h_raw; y++)
{
for (int x = 0; x < lineNum; x++)
{
SSG_featureClusteringInfo& a_featureInfo = featureInfoMask[x][y];
if ((0 == a_featureInfo.featurType) || (a_featureInfo.clusterID > 0)) //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ѿ<EFBFBD><D1BE><EFBFBD><EFBFBD><EFBFBD>
continue;
SVzNL3DPoint& a_feature3DValue = feature3DInfo[x][y];
SVzNL3DRangeD a_clusterRoi;
a_clusterRoi.xRange.min = a_feature3DValue.x;
a_clusterRoi.xRange.max = a_feature3DValue.x;
a_clusterRoi.yRange.min = a_feature3DValue.y;
a_clusterRoi.yRange.max = a_feature3DValue.y;
a_clusterRoi.zRange.min = a_feature3DValue.z;
a_clusterRoi.zRange.max = a_feature3DValue.z;
SVzNL2DPoint a_seedPos = { x, y };
std::vector< SVzNL2DPoint> a_cluster;
a_cluster.push_back(a_seedPos);
wd_gridPointClustering(
featureInfoMask,//int<6E><74><EFBFBD><EFBFBD>¼<EFBFBD><C2BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ǻ<EFBFBD>clusterID<49><44><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>flag
feature3DInfo,//double,<2C><>¼<EFBFBD><C2BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϣ
clusterCheckWin, //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
growParam,//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
clusterID, //<2F><>ǰCluster<65><72>ID
a_cluster, //result
a_clusterRoi
);
clusters.push_back(a_cluster);
clustersRoi3D.push_back(a_clusterRoi);
clusterID++;
}
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
std::vector<int> validCluserIndexing;
int clusterSize = (int)clusters.size();
for (int i = 0; i < clusterSize; i++)
{
SVzNL3DRangeD& a_roi = clustersRoi3D[i];
double L = a_roi.xRange.max - a_roi.xRange.min;
double W = a_roi.yRange.max - a_roi.yRange.min;
if ((L > workpiecePara.holeDiameter * 0.5) && (L < workpiecePara.holeDiameter * 2) &&
(W > workpiecePara.holeDiameter * 0.5) && (W < workpiecePara.holeDiameter * 2))
validCluserIndexing.push_back(i);
}
//<2F><><EFBFBD>ɽ<EFBFBD><C9BD><EFBFBD>
std::vector< SWD_HoleInfo> holes;
int objectSize = (int)validCluserIndexing.size();
for (int objIdx = 0; objIdx < objectSize; objIdx++)
{
std::vector<SVzNL3DPoint> pointArray;
int clusterIdx = validCluserIndexing[objIdx];
//ȡcluster<65>ϵĵ<CFB5>
int clusterPtSize = (int)clusters[clusterIdx].size();
double minZ = DBL_MAX;
for (int i = 0; i < clusterPtSize; i++)
{
SVzNL2DPoint a_pos = clusters[clusterIdx][i];
SSG_featureClusteringInfo& a_featureInfo = featureInfoMask[a_pos.x][a_pos.y];
int lineIdx = a_featureInfo.lineIdx;
int ptIdx = a_featureInfo.ptIdx;
SVzNL3DPoint a_pt3d = scanLines[lineIdx][ptIdx].pt3D;
if (minZ > a_pt3d.z)
minZ = a_pt3d.z;
pointArray.push_back(a_pt3d);
}
//Բ<><D4B2><EFBFBD><EFBFBD>
SVzNL3DPoint center;
double radius;
double err = fitCircleByLeastSquare(pointArray, center, radius);
center.z = minZ;
SWD_HoleInfo a_hole;
a_hole.center = { center.x, center.y, center.z };
a_hole.radius = radius;
holes.push_back(a_hole);
}
//<2F>ָ<EFBFBD>
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>W<EFBFBD><57><EFBFBD>ӽ<EFBFBD><D3BD>ĵ㣬Ȼ<E3A3AC><C8BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD>L<EFBFBD><4C><EFBFBD>ӽ<EFBFBD><D3BD>ĵ<EFBFBD>
double distDeviation = 5.0; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ĺϸ<C4BA><CFB8><EFBFBD><EFBFBD>ޡ<EFBFBD>С<EFBFBD>ڴ˾<DAB4><CBBE>룬<EFBFBD><EBA3AC>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF>Ϊ<EFBFBD><CEAA>Ч
for (int objIdx = 0; objIdx < objectSize; objIdx++)
{
if (holes[objIdx].radius < 0)
continue;
holes[objIdx].radius = -1;
SWD_HoleInfo& p0 = holes[objIdx];
int idx1 = distanceSearchObject(p0.center, holes, workpiecePara.holeDist_W, distDeviation);
if (idx1 < 0)
continue;
SVzNLRangeD angleRange = { 85, 95 }; //<2F><>ֱ<EFBFBD><D6B1>5<EFBFBD>ȷ<EFBFBD>Χ
SWD_HoleInfo& p1 = holes[idx1];
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ӽ<EFBFBD>distance<63>ҽǶ<D2BD>Ϊangle<6C><65>Ŀ<EFBFBD><C4BF>, <20>ԽǶ<D4BD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD>
int idx2 = angleConditionDistanceSearch(
p0.center, p1.center,
holes,
workpiecePara.holeDist_L, distDeviation,
angleRange);
if (idx2 < 0)
continue;
SWD_HoleInfo& p2 = holes[idx2];
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ӽ<EFBFBD>distance<63>ҽǶ<D2BD>Ϊangle<6C><65>Ŀ<EFBFBD><C4BF>, <20>ԽǶ<D4BD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD>
int idx3 = angleConditionDistanceSearch(
p1.center, p0.center,
holes,
workpiecePara.holeDist_L, distDeviation,
angleRange);
if (idx3 < 0)
continue;
SWD_HoleInfo& p3 = holes[idx3];
p1.radius = -1;
p2.radius = -1;
p3.radius = -1;
//<2F><><EFBFBD>¼<EFBFBD><C2BC><EFBFBD>Zֵ<5A><D6B5><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD>׵<EFBFBD>ԭ<EFBFBD><D4AD><EFBFBD><EFBFBD>Zֵ<5A>᲻׼ȷ<D7BC><C8B7>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD>ߵ<EFBFBD><DFB5>е㴦<D0B5><E3B4A6>Zֵ<5A>ľ<EFBFBD>ֵ<EFBFBD><D6B5>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Zֵ
workpieceHolePositioning v1.0.1 初始发布版本。
2026-01-26 22:36:13 +08:00
SVzNL3DPoint center_p0p1 = { (p0.center.x + p1.center.x) / 2,(p0.center.y + p1.center.y) / 2, (p0.center.z + p1.center.z) / 2 };
SVzNL3DPoint center_p0p2 = { (p0.center.x + p2.center.x) / 2,(p0.center.y + p2.center.y) / 2, (p0.center.z + p2.center.z) / 2 };
SVzNL3DPoint center_p1p3 = { (p1.center.x + p3.center.x) / 2,(p1.center.y + p3.center.y) / 2, (p1.center.z + p3.center.z) / 2 };
SVzNL3DPoint center_p2p3 = { (p2.center.x + p3.center.x) / 2,(p2.center.y + p3.center.y) / 2, (p2.center.z + p3.center.z) / 2 };
double rectR = 5.0;
double z1 = _getMeanZ(quantiValue, center_p0p1, roi2D, rectR);
double z2 = _getMeanZ(quantiValue, center_p0p2, roi2D, rectR);
double z3 = _getMeanZ(quantiValue, center_p1p3, roi2D, rectR);
double z4 = _getMeanZ(quantiValue, center_p2p3, roi2D, rectR);
p0.center.z = (z1 + z2) / 2;
p1.center.z = (z1 + z3) / 2;
p2.center.z = (z2 + z4) / 2;
p3.center.z = (z3 + z4) / 2;
workpieceHolePositioning v1.0.0 初始版本提交
2026-01-26 15:57:19 +08:00
WD_workpieceInfo a_workpiece;
a_workpiece.workpieceType = workpiecePara.workpieceType;
a_workpiece.holes.push_back(p0.center);
a_workpiece.holes.push_back(p1.center);
a_workpiece.holes.push_back(p2.center);
a_workpiece.holes.push_back(p3.center);
workpieceHolePositioning v1.0.1 初始发布版本。
2026-01-26 22:36:13 +08:00
for (int m = 0; m < 4; m++)
{
SVzNL3DPoint a_pt = a_workpiece.holes[m];
a_pt.z = a_pt.z + 20; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD>Է<EFBFBD><D4B7><EFBFBD>ֻ<EFBFBD><D6BB>z<EFBFBD><7A>
a_workpiece.holesDir.push_back(a_pt);
}
workpieceHolePositioning v1.0.0 初始版本提交
2026-01-26 15:57:19 +08:00
a_workpiece.center = { (p0.center.x + p1.center.x + p2.center.x + p3.center.x) / 4,
(p0.center.y + p1.center.y + p2.center.y + p3.center.y) / 4,
workpieceHolePositioning v1.0.1 初始发布版本。
2026-01-26 22:36:13 +08:00
(z1 + z2 + z3 + z4) / 4 };
workpieceHolePositioning v1.0.0 初始版本提交
2026-01-26 15:57:19 +08:00
SVzNL3DPoint y_dir;
if (p0.center.y < p2.center.y)
workpieceHolePositioning v1.0.1 初始发布版本。
2026-01-26 22:36:13 +08:00
y_dir = { p0.center.x - p2.center.x, p0.center.y - p2.center.y, 0 };
workpieceHolePositioning v1.0.0 初始版本提交
2026-01-26 15:57:19 +08:00
else
workpieceHolePositioning v1.0.1 初始发布版本。
2026-01-26 22:36:13 +08:00
y_dir = { p2.center.x - p0.center.x, p2.center.y - p0.center.y, 0 };
double modLen = sqrt(pow(y_dir.x, 2) + pow(y_dir.y, 2));
y_dir = { y_dir.x / modLen, y_dir.y / modLen, 0 };
a_workpiece.y_dir = { y_dir.x * 20 + a_workpiece.center.x, y_dir.y * 20 + a_workpiece.center.y, a_workpiece.center.z };
a_workpiece.z_dir = { a_workpiece.center.x, a_workpiece.center.y, a_workpiece.center.z - 20 };
workpieceHolePositioning v1.0.1 添加了工件姿态（欧拉角）输出
2026-01-30 00:10:14 +08:00
workpieceHolePositioning v1.0.0 初始版本提交
2026-01-26 15:57:19 +08:00
workpiecePositioning.push_back(a_workpiece);
}
int workpieceNum = (int)workpiecePositioning.size();
//<2F><>ת<EFBFBD><D7AA>ȥ
for (int i = 0; i < workpieceNum; i++)
{
SVzNL3DPoint rpt;
rpt = _ptRotate(workpiecePositioning[i].center, groundCalibPara.invRMatrix);
workpiecePositioning[i].center = rpt;
rpt = _ptRotate(workpiecePositioning[i].y_dir, groundCalibPara.invRMatrix);
workpiecePositioning[i].y_dir = rpt;
rpt = _ptRotate(workpiecePositioning[i].z_dir, groundCalibPara.invRMatrix);
workpiecePositioning[i].z_dir = rpt;
for (int j = 0, j_max = (int)workpiecePositioning[i].holes.size(); j < j_max; j++)
{
rpt = _ptRotate(workpiecePositioning[i].holes[j], groundCalibPara.invRMatrix);
workpiecePositioning[i].holes[j] = rpt;
workpieceHolePositioning v1.0.1 初始发布版本。
2026-01-26 22:36:13 +08:00
rpt = _ptRotate(workpiecePositioning[i].holesDir[j], groundCalibPara.invRMatrix);
workpiecePositioning[i].holesDir[j] = rpt;
workpieceHolePositioning v1.0.0 初始版本提交
2026-01-26 15:57:19 +08:00
}
workpieceHolePositioning v1.0.1 添加了工件姿态（欧拉角）输出
2026-01-30 00:10:14 +08:00
SVzNL3DPoint vector_z = { workpiecePositioning[i].z_dir.x - workpiecePositioning[i].center.x,
workpiecePositioning[i].z_dir.y - workpiecePositioning[i].center.y,
workpiecePositioning[i].z_dir.z - workpiecePositioning[i].center.z };
SVzNL3DPoint vector_y = { workpiecePositioning[i].y_dir.x - workpiecePositioning[i].center.x,
workpiecePositioning[i].y_dir.y - workpiecePositioning[i].center.y,
workpiecePositioning[i].y_dir.z - workpiecePositioning[i].center.z };
double mod_vz = sqrt(pow(vector_z.x, 2) + pow(vector_z.y, 2) + pow(vector_z.z, 2));
vector_z = { vector_z.x / mod_vz, vector_z.y / mod_vz, vector_z.z / mod_vz }; //<2F><>һ<EFBFBD><D2BB>
double mod_vy = sqrt(pow(vector_y.x, 2) + pow(vector_y.y, 2) + pow(vector_y.z, 2));
vector_y = { vector_y.x / mod_vy, vector_y.y / mod_vy, vector_y.z / mod_vy }; //<2F><>һ<EFBFBD><D2BB>
//<2F><><EFBFBD>˳<EFBFBD>vector_x
SVzNL3DPoint vector_x;
vector_x.x = vector_y.y * vector_z.z - vector_z.y * vector_y.z;
vector_x.y = vector_y.z * vector_z.x - vector_z.z * vector_y.x;
vector_x.z = vector_y.x * vector_z.y - vector_z.x * vector_y.y;
workpieceHolePositioning v1.1.0 对工件姿态规范化为中心点（操作点）加三个方向矢量
2026-01-31 14:36:54 +08:00
workpiecePositioning[i].x_dir = vector_x;
workpiecePositioning[i].y_dir = vector_y;
workpiecePositioning[i].z_dir = vector_z;
#if 0
workpieceHolePositioning v1.0.1 添加了工件姿态（欧拉角）输出
2026-01-30 00:10:14 +08:00
//<2F>õ<EFBFBD><C3B5><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>
double R[3][3];
R[0][0] = vector_x.x;
R[1][0] = vector_x.y;
R[2][0] = vector_x.z;
R[0][1] = vector_y.x;
R[1][1] = vector_y.y;
R[2][1] = vector_y.z;
R[0][2] = vector_z.x;
R[1][2] = vector_z.y;
R[2][2] = vector_z.z;
SSG_EulerAngles eulerAngle = rotationMatrixToEulerZYX(R);
workpiecePositioning[i].workpiecePose = eulerAngle;
workpieceHolePositioning v1.1.0 对工件姿态规范化为中心点（操作点）加三个方向矢量
2026-01-31 14:36:54 +08:00
#endif
workpieceHolePositioning v1.0.0 初始版本提交
2026-01-26 15:57:19 +08:00
}
return;
}
Reference in New Issue Copy Permalink