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

1344 lines
40 KiB
C++
Raw Normal View History

rodAndBarDetection Initial check in
2026-01-11 17:04:06 +08:00
#include <vector>
#include "SG_baseDataType.h"
#include "SG_baseAlgo_Export.h"
#include "rodAndBarDetection_Export.h"
#include <opencv2/opencv.hpp>
#include <limits>
//version 1.0.0 : base version release to customer
rodAarcFeatueDetection version 1.1.0 : 添加了地面调平和棒材定位
2026-03-21 23:15:43 +08:00
//version 1.1.0 : <20><><EFBFBD><EFBFBD><EFBFBD>˵<EFBFBD><CBB5><EFBFBD><EFBFBD><EFBFBD>ƽ<EFBFBD>Ͱ<EFBFBD><CDB0>Ķ<EFBFBD>λ
rodAarcFeatueDetection version 1.1.1 : 初始发布给客户的版本
2026-03-22 09:49:06 +08:00
//version 1.1.1 : <20><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͻ<EFBFBD><CDBB>İ汾
rodAndBarDetection version 1.2.0 : 配天螺杆测量增加了定位盘中心测量功能
2026-04-14 15:04:46 +08:00
//version 1.2.0 : <20><><EFBFBD><EFBFBD><EFBFBD>ݸ˲<DDB8><CBB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD>λ<EFBFBD><CEBB><EFBFBD><EFBFBD><EFBFBD>IJ<EFBFBD><C4B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
std::string m_strVersion = "1.2.0";
rodAndBarDetection Initial check in
2026-01-11 17:04:06 +08:00
const char* wd_rodAndBarDetectionVersion(void)
{
return m_strVersion.c_str();
}
rodAarcFeatueDetection version 1.1.0 : 添加了地面调平和棒材定位
2026-03-21 23:15:43 +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_rodPosition_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_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;
}
rodAndBarDetection code update。在调试前备份算法代码
2026-01-11 21:05:08 +08:00
SVzNL3DPoint getArcPeak(
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
rodAndBarDetection version 1.0.0 初始版本
2026-01-12 17:27:50 +08:00
SWD_segFeature & a_arcFeature,
SVzNL2DPoint& arcPos)
rodAndBarDetection code update。在调试前备份算法代码
2026-01-11 21:05:08 +08:00
{
SVzNL3DPoint arcPeak = scanLines[a_arcFeature.lineIdx][a_arcFeature.startPtIdx].pt3D;
for (int i = a_arcFeature.startPtIdx+1; i <= a_arcFeature.endPtIdx; i++)
{
if (scanLines[a_arcFeature.lineIdx][i].pt3D.z > 1e-4) //<2F><><EFBFBD><EFBFBD><EFBFBD>յ<EFBFBD>
{
if (arcPeak.z > scanLines[a_arcFeature.lineIdx][i].pt3D.z)
rodAndBarDetection version 1.0.0 初始版本
2026-01-12 17:27:50 +08:00
{
rodAndBarDetection code update。在调试前备份算法代码
2026-01-11 21:05:08 +08:00
arcPeak = scanLines[a_arcFeature.lineIdx][i].pt3D;
rodAndBarDetection version 1.0.0 初始版本
2026-01-12 17:27:50 +08:00
arcPos = { a_arcFeature.lineIdx , i };
}
}
}
return arcPeak;
}
SVzNL3DPoint getArcPeak_parabolaFitting(
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
SWD_segFeature& a_arcFeature,
SVzNL2DPoint& arcPos)
{
std::vector<cv::Point2d> points;
for (int i = a_arcFeature.startPtIdx + 1; i <= a_arcFeature.endPtIdx; i++)
{
if (scanLines[a_arcFeature.lineIdx][i].pt3D.z > 1e-4) //<2F><><EFBFBD><EFBFBD><EFBFBD>յ<EFBFBD>
{
cv::Point2d a_pt2D;
if (scanLines[a_arcFeature.lineIdx][i].pt3D.z > 1e-4)
{
a_pt2D.x = scanLines[a_arcFeature.lineIdx][i].pt3D.y;
a_pt2D.y = scanLines[a_arcFeature.lineIdx][i].pt3D.z;
points.push_back(a_pt2D);
}
}
}
double a, b, c, mse, max_err;
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> y = ax ^ 2 + bx + c
bool result = leastSquareParabolaFitEigen(
points,
a, b, c,
mse, max_err);
double yP = -b / (2 * a);
//Ѱ<><D1B0><EFBFBD><EFBFBD>yP<79><50><EFBFBD><EFBFBD><EFBFBD>ĵ<EFBFBD><C4B5><EFBFBD>ΪPeak<61><6B>
SVzNL3DPoint arcPeak;
double minDist = -1;
for (int i = a_arcFeature.startPtIdx + 1; i <= a_arcFeature.endPtIdx; i++)
{
if (scanLines[a_arcFeature.lineIdx][i].pt3D.z > 1e-4) //<2F><><EFBFBD><EFBFBD><EFBFBD>յ<EFBFBD>
{
double dist = abs(scanLines[a_arcFeature.lineIdx][i].pt3D.y - yP);
if (minDist < 0)
{
minDist = dist;
arcPeak = scanLines[a_arcFeature.lineIdx][i].pt3D;
arcPos = { a_arcFeature.lineIdx , i };
}
else
{
if(minDist > dist)
{
minDist = dist;
arcPeak = scanLines[a_arcFeature.lineIdx][i].pt3D;
arcPos = { a_arcFeature.lineIdx , i };
}
}
rodAndBarDetection code update。在调试前备份算法代码
2026-01-11 21:05:08 +08:00
}
}
return arcPeak;
}
//ͶӰ<CDB6><D3B0><EFBFBD><EFBFBD>ȡROI<4F>ڵ<EFBFBD><DAB5><EFBFBD><EFBFBD><EFBFBD>
void xoyROIProjection(
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
const double* rtMatrix,
SSG_ROIRectD& roi_xoy,
std::vector<SVzNL3DPoint>& projectPoints
)
{
int lineNum = (int)scanLines.size();
for (int line = 0; line < lineNum; line++)
{
std::vector<SVzNL3DPosition>& a_line = scanLines[line];
int ptNum = (int)a_line.size();
for (int i = 0; i < (int)a_line.size(); i++)
{
SVzNL3DPoint a_pt = a_line[i].pt3D;
if (a_pt.z < 1e-4)
continue;
double x = a_pt.x * rtMatrix[0] + a_pt.y * rtMatrix[1] + a_pt.z * rtMatrix[2];
double y = a_pt.x * rtMatrix[3] + a_pt.y * rtMatrix[4] + a_pt.z * rtMatrix[5];
double z = a_pt.x * rtMatrix[6] + a_pt.y * rtMatrix[7] + a_pt.z * rtMatrix[8];
if ((x >= roi_xoy.left) && (x <= roi_xoy.right) &&
(y >= roi_xoy.top) && (y <= roi_xoy.bottom))
{
a_pt.x = x;
a_pt.y = y;
a_pt.z = z;
projectPoints.push_back(a_pt);
}
}
}
}
SVzNLRangeD getZRange(std::vector<SVzNL3DPoint>& projectPoints)
{
int ptNum = (int)projectPoints.size();
SVzNLRangeD zRange;
zRange.min = DBL_MAX;
zRange.max = DBL_MIN;
for (int i = 0; i < ptNum; i++)
{
zRange.min = zRange.min > projectPoints[i].z ? projectPoints[i].z : zRange.min;
zRange.max = zRange.max < projectPoints[i].z ? projectPoints[i].z : zRange.max;
}
return zRange;
}
void zCutPointClouds(
std::vector<SVzNL3DPoint>& projectPoints,
SVzNLRangeD& zRange,
std::vector<SVzNL3DPoint>& cutLayerPoints)
{
int ptNum = (int)projectPoints.size();
for (int i = 0; i < ptNum; i++)
{
if ((projectPoints[i].z >= zRange.min) && (projectPoints[i].z <= zRange.max))
cutLayerPoints.push_back(projectPoints[i]);
}
}
SVzNL3DPoint getXoYCentroid(std::vector<SVzNL3DPoint>& points)
{
int ptNum = (int)points.size();
SVzNL3DPoint centroid = { 0.0, 0.0, 0.0 };
if (ptNum == 0)
return centroid;
for (int i = 0; i < ptNum; i++)
{
centroid.x += points[i].x;
centroid.y += points[i].y;
}
centroid.x = centroid.x / ptNum;
centroid.y = centroid.y / ptNum;
return centroid;
}
SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, 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;
}
rodAndBarDetection version 1.2.0 : 配天螺杆测量增加了定位盘中心测量功能
2026-04-14 15:04:46 +08:00
//<2F><><EFBFBD><EFBFBD><EFBFBD>ݸ˶˲<CBB6><CBB2><EFBFBD><EFBFBD>ĵ<EFBFBD>λ<EFBFBD><CEBB>
rodAndBarDetection Initial check in
2026-01-11 17:04:06 +08:00
void sx_hexHeadScrewMeasure(
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
bool isHorizonScan, //true:<3A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƽ<EFBFBD>в۵<D0B2><DBB5><EFBFBD>false:<3A><><EFBFBD><EFBFBD><EFBFBD>ߴ<EFBFBD>ֱ<EFBFBD>۵<EFBFBD>
const SSG_cornerParam cornerPara,
const SSG_outlierFilterParam filterParam,
const SSG_treeGrowParam growParam,
rodAndBarDetection version 1.0.0 初始版本
2026-01-12 17:27:50 +08:00
double rodDiameter,
rodAndBarDetection version 1.2.0 : 配天螺杆测量增加了定位盘中心测量功能
2026-04-14 15:04:46 +08:00
std::vector<SSX_rodPoseInfo>& screwInfo,
rodAndBarDetection Initial check in
2026-01-11 17:04:06 +08:00
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();
//<2F>ж<EFBFBD><D0B6><EFBFBD><EFBFBD>ݸ<EFBFBD>ʽ<EFBFBD>Ƿ<EFBFBD>Ϊgrid<69><64><EFBFBD>㷨ֻ<E3B7A8>ܴ<EFBFBD><DCB4><EFBFBD>grid<69><64><EFBFBD>ݸ<EFBFBD>ʽ
bool isGridData = true;
for (int line = 0; line < lineNum; line++)
{
if (linePtNum != (int)scanLines[line].size())
{
isGridData = false;
break;
}
}
if (false == isGridData)//<2F><><EFBFBD>ݲ<EFBFBD><DDB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʽ
{
*errCode = SG_ERR_NOT_GRID_FORMAT;
return;
}
std::vector< std::vector<SVzNL3DPosition>> data_lines;
if (false == isHorizonScan)
{
data_lines.resize(lineNum);
for (int line = 0; line < lineNum; line++)
{
data_lines[line].insert(data_lines[line].end(), scanLines[line].begin(), scanLines[line].end());
for (int j = 0, j_max = (int)data_lines[line].size(); j < j_max; j++)
{
data_lines[line][j].nPointIdx = j;
scanLines[line][j].nPointIdx = 0; //ת<>帴<EFBFBD><E5B8B4>
}
}
}
else
{
data_lines.resize(linePtNum);
for (int i = 0; i < linePtNum; i++)
data_lines[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>
data_lines[j][line] = scanLines[line][j];
data_lines[j][line].pt3D.x = scanLines[line][j].pt3D.y;
data_lines[j][line].pt3D.y = scanLines[line][j].pt3D.x;
}
}
lineNum = linePtNum;
linePtNum = (int)data_lines[0].size();
for (int line = 0; line < lineNum; line++)
{
for (int j = 0, j_max = (int)data_lines[line].size(); j < j_max; j++)
data_lines[line][j].nPointIdx = j;
}
}
std::vector<std::vector<SWD_segFeature>> arcFeatures;
for (int line = 0; line < lineNum; line++)
{
rodAndBarDetection version 1.0.0 初始版本
2026-01-12 17:27:50 +08:00
if (line == 329)
rodAndBarDetection Initial check in
2026-01-11 17:04:06 +08:00
int kkk = 1;
std::vector<SVzNL3DPosition>& lineData = data_lines[line];
//<2F>˲<EFBFBD><CBB2><EFBFBD><EFBFBD>˳<EFBFBD><CBB3>쳣<EFBFBD><ECB3A3>
sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam);
std::vector<SWD_segFeature> line_ringArcs;
int dataSize = (int)lineData.size();
rodAndBarDetection version 1.0.0 初始版本
2026-01-12 17:27:50 +08:00
SVzNLRangeD arcWidth;
arcWidth.min = rodDiameter / 2;
arcWidth.max = rodDiameter * 1.5;
rodAndBarDetection Initial check in
2026-01-11 17:04:06 +08:00
//<2F><>ȡArc<72><63><EFBFBD><EFBFBD>
wd_getRingArcFeature(
lineData,
line, //<2F><>ǰɨ<C7B0><C9A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
cornerPara,
rodAndBarDetection version 1.0.0 初始版本
2026-01-12 17:27:50 +08:00
arcWidth, //<2F><><EFBFBD><EFBFBD><EFBFBD>ȣ<EFBFBD><C8A3>԰뾶Ϊ<EBBEB6><CEAA>׼<EFBFBD><D7BC><EFBFBD><EFBFBD>Ӧ60<36>Ƚ<EFBFBD>
rodAndBarDetection Initial check in
2026-01-11 17:04:06 +08:00
line_ringArcs //<2F><>
);
arcFeatures.push_back(line_ringArcs);
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
std::vector<SWD_segFeatureTree> growTrees;
wd_getSegFeatureGrowingTrees(
arcFeatures,
growTrees,
growParam);
if (growTrees.size() == 0)
{
*errCode = SG_ERR_NOT_GRID_FORMAT;
return;
}
int objNum = (int)growTrees.size();
rodAndBarDetection version 1.0.0 初始版本
2026-01-12 17:27:50 +08:00
//<2F>ñ<EFBFBD>־<EFBFBD><D6BE><EFBFBD><EFBFBD><EFBFBD><EFBFBD>debug
for (int i = 0; i < objNum; i++)
{
int nodeNum = (int)growTrees[i].treeNodes.size();
for (int j = 0; j < nodeNum; j++)
{
int lineIdx, ptIdx;
if (false == isHorizonScan)
{
lineIdx = growTrees[i].treeNodes[j].lineIdx;
for (int m = growTrees[i].treeNodes[j].startPtIdx; m <= growTrees[i].treeNodes[j].endPtIdx; m++)
{
ptIdx = m;
scanLines[lineIdx][ptIdx].nPointIdx = 1;
}
}
else
{
ptIdx = growTrees[i].treeNodes[j].lineIdx;
for (int m = growTrees[i].treeNodes[j].startPtIdx; m <= growTrees[i].treeNodes[j].endPtIdx; m++)
{
lineIdx = m;
scanLines[lineIdx][ptIdx].nPointIdx = 1;
}
}
}
}
//<2F><><EFBFBD><EFBFBD>Ŀ<EFBFBD>괦<EFBFBD><EAB4A6>
rodAndBarDetection Initial check in
2026-01-11 17:04:06 +08:00
for (int i = 0; i < objNum; i++)
{
//<2F>ռ<EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
rodAndBarDetection code update。在调试前备份算法代码
2026-01-11 21:05:08 +08:00
std::vector<SVzNL3DPoint> fitPoints;
rodAndBarDetection version 1.0.0 初始版本
2026-01-12 17:27:50 +08:00
std::vector<SVzNL2DPoint> fit2DPos;
rodAndBarDetection code update。在调试前备份算法代码
2026-01-11 21:05:08 +08:00
int nodeSize = (int)growTrees[i].treeNodes.size();
for (int j = 0; j < nodeSize; j++)
rodAndBarDetection Initial check in
2026-01-11 17:04:06 +08:00
{
rodAndBarDetection version 1.0.0 初始版本
2026-01-12 17:27:50 +08:00
SVzNL2DPoint arcPos;
SVzNL3DPoint a_pt = getArcPeak_parabolaFitting(data_lines, growTrees[i].treeNodes[j], arcPos);
//SVzNL3DPoint a_pt = getArcPeak(data_lines, growTrees[i].treeNodes[j], arcPos);
rodAndBarDetection code update。在调试前备份算法代码
2026-01-11 21:05:08 +08:00
fitPoints.push_back(a_pt);
rodAndBarDetection version 1.0.0 初始版本
2026-01-12 17:27:50 +08:00
fit2DPos.push_back(arcPos);
rodAndBarDetection Initial check in
2026-01-11 17:04:06 +08:00
}
rodAndBarDetection version 1.0.0 初始版本
2026-01-12 17:27:50 +08:00
if (fitPoints.size() < 27)
rodAndBarDetection code update。在调试前备份算法代码
2026-01-11 21:05:08 +08:00
continue;
//ȥ<><C8A5>ͷβ<CDB7><CEB2>5<EFBFBD><35><EFBFBD>㣬<EFBFBD><E3A3AC>ֹ<EFBFBD>ڶ˲<DAB6><CBB2>͸<EFBFBD><CDB8><EFBFBD>ɨ<EFBFBD><C9A8>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>и<EFBFBD><D0B8><EFBFBD>
rodAndBarDetection version 1.0.0 初始版本
2026-01-12 17:27:50 +08:00
fitPoints.erase(fitPoints.begin(), fitPoints.begin() + 10);
fit2DPos.erase(fit2DPos.begin(), fit2DPos.begin() + 10);
rodAndBarDetection code update。在调试前备份算法代码
2026-01-11 21:05:08 +08:00
fitPoints.erase(fitPoints.end() - 5, fitPoints.end());
rodAndBarDetection version 1.0.0 初始版本
2026-01-12 17:27:50 +08:00
fit2DPos.erase(fit2DPos.end() - 5, fit2DPos.end());
//<2F>ñ<EFBFBD>־
for (int j = 0; j < (int)fit2DPos.size(); j++)
{
int lineIdx, ptIdx;
if (false == isHorizonScan)
{
lineIdx = fit2DPos[j].x;
ptIdx = fit2DPos[j].y;
}
else
{
lineIdx = fit2DPos[j].y;
ptIdx = fit2DPos[j].x;
}
scanLines[lineIdx][ptIdx].nPointIdx = 2;
}
rodAndBarDetection code update。在调试前备份算法代码
2026-01-11 21:05:08 +08:00
//<2F><><EFBFBD><EFBFBD>
SVzNL3DPoint P0_center, P1_dir;
bool result = fitLine3DLeastSquares(fitPoints, P0_center, P1_dir);
if (false == result)
continue;
//ͶӰ
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>
SVzNL3DPoint vector1 = P1_dir;
SVzNL3DPoint vector2 = { 0, 0, -1.0 };
rodAndBarDetection code update: fix bugs for compile errors
2026-01-11 21:19:52 +08:00
SSG_planeCalibPara rotatePara = wd_computeRTMatrix( vector1, vector2);
rodAndBarDetection code update。在调试前备份算法代码
2026-01-11 21:05:08 +08:00
//
rodAndBarDetection version 1.0.0 初始版本
2026-01-12 17:27:50 +08:00
SVzNL3DPoint P0_rotate = _ptRotate(P0_center, rotatePara.planeCalib);
rodAndBarDetection code update。在调试前备份算法代码
2026-01-11 21:05:08 +08:00
SSG_ROIRectD roi_xoy;
rodAndBarDetection version 1.0.0 初始版本
2026-01-12 17:27:50 +08:00
roi_xoy.left = P0_rotate.x - rodDiameter * 2; //2D<32><44>Χ
roi_xoy.right = P0_rotate.x + rodDiameter * 2; //2D<32><44>Χ
roi_xoy.top = P0_rotate.y - rodDiameter * 2; //2D<32><44>Χ
roi_xoy.bottom = P0_rotate.y + rodDiameter * 2; //2D<32><44>Χ
#if 1
std::vector< SVzNL3DPoint> verifyData;
for (int m = 0; m < (int)fitPoints.size(); m++)
{
SVzNL3DPoint rPt = _ptRotate(fitPoints[m], rotatePara.planeCalib);
verifyData.push_back(rPt);
}
#endif
rodAndBarDetection code update。在调试前备份算法代码
2026-01-11 21:05:08 +08:00
std::vector< SVzNL3DPoint> roiProjectionData;
rodAndBarDetection version 1.0.0 初始版本
2026-01-12 17:27:50 +08:00
xoyROIProjection(data_lines, rotatePara.planeCalib, roi_xoy, roiProjectionData);
rodAndBarDetection code update。在调试前备份算法代码
2026-01-11 21:05:08 +08:00
//ȡ<><C8A1><EFBFBD><EFBFBD>
SVzNLRangeD zRange = getZRange(roiProjectionData);
SVzNLRangeD cutZRange;
cutZRange.min = zRange.min;
rodAndBarDetection version 1.0.0 初始版本
2026-01-12 17:27:50 +08:00
cutZRange.max = zRange.min + 5.0; //5mm<6D>Ķ<EFBFBD><C4B6><EFBFBD>
rodAndBarDetection code update。在调试前备份算法代码
2026-01-11 21:05:08 +08:00
std::vector<SVzNL3DPoint> surfacePoints;
zCutPointClouds(roiProjectionData, cutZRange, surfacePoints);
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ĵ<EFBFBD>
rodAndBarDetection version 1.0.0 初始版本
2026-01-12 17:27:50 +08:00
SVzNL3DPoint projectionCenter;// = getXoYCentroid(surfacePoints);
SVzNL3DRangeD roi3D = wd_getPointCloudROI(surfacePoints);
projectionCenter.x = (roi3D.xRange.min + roi3D.xRange.max) / 2;
projectionCenter.y = (roi3D.yRange.min + roi3D.yRange.max) / 2;
rodAndBarDetection code update。在调试前备份算法代码
2026-01-11 21:05:08 +08:00
projectionCenter.z = zRange.min;
//<2F><>ת<EFBFBD><D7AA>ԭ<EFBFBD><D4AD><EFBFBD><EFBFBD>ϵ
SVzNL3DPoint surfaceCenter = _ptRotate(projectionCenter, rotatePara.invRMatrix);
//<2F><><EFBFBD><EFBFBD>Rod<6F><64>Ϣ
rodAndBarDetection version 1.2.0 : 配天螺杆测量增加了定位盘中心测量功能
2026-04-14 15:04:46 +08:00
SSX_rodPoseInfo a_rod;
rodAndBarDetection code update。在调试前备份算法代码
2026-01-11 21:05:08 +08:00
a_rod.center = surfaceCenter;
a_rod.axialDir = P1_dir;
screwInfo.push_back(a_rod);
rodAndBarDetection Initial check in
2026-01-11 17:04:06 +08:00
}
rodAndBarDetection code update。在调试前备份算法代码
2026-01-11 21:05:08 +08:00
if (true == isHorizonScan)
rodAndBarDetection Initial check in
2026-01-11 17:04:06 +08:00
{
rodAndBarDetection code update。在调试前备份算法代码
2026-01-11 21:05:08 +08:00
int objNum = (int)screwInfo.size();
for (int i = 0; i < objNum; i++)
rodAndBarDetection Initial check in
2026-01-11 17:04:06 +08:00
{
rodAndBarDetection code update。在调试前备份算法代码
2026-01-11 21:05:08 +08:00
double tmp = screwInfo[i].center.x;
screwInfo[i].center.x = screwInfo[i].center.y;
screwInfo[i].center.y = tmp;
tmp = screwInfo[i].axialDir.x;
screwInfo[i].axialDir.x = screwInfo[i].axialDir.y;
screwInfo[i].axialDir.y = tmp;
//screwInfo[i].rotateAngle += 90;
rodAndBarDetection Initial check in
2026-01-11 17:04:06 +08:00
}
}
rodAndBarDetection code update。在调试前备份算法代码
2026-01-11 21:05:08 +08:00
return;
rodAndBarDetection Initial check in
2026-01-11 17:04:06 +08:00
}
rodAndBarDetection version 1.2.0 : 配天螺杆测量增加了定位盘中心测量功能
2026-04-14 15:04:46 +08:00
double _getListMeanZ(std::vector< SVzNL3DPosition>& listData)
{
if (listData.size() == 0)
return 0;
double meanZ = 0;
for (int i = 0; i < (int)listData.size(); i++)
meanZ += listData[i].pt3D.z;
meanZ = meanZ / (double)listData.size();
return meanZ;
}
SSG_ROIRectD _getListROI(std::vector< SVzNL3DPosition>& listData)
{
if (listData.size() == 0)
return { 0,0,0,0 };
SSG_ROIRectD roi = { listData[0].pt3D.x, listData[0].pt3D.x, listData[0].pt3D.y, listData[0].pt3D.y};
for (int i = 0; i < (int)listData.size(); i++)
{
roi.left = roi.left > listData[i].pt3D.x ? listData[i].pt3D.x : roi.left;
roi.right = roi.right < listData[i].pt3D.x ? listData[i].pt3D.x : roi.right;
roi.top = roi.top > listData[i].pt3D.y ? listData[i].pt3D.y : roi.top;
roi.bottom = roi.bottom < listData[i].pt3D.y ? listData[i].pt3D.y : roi.bottom;
}
return roi;
}
//<2F><><EFBFBD>㶨λ<E3B6A8><CEBB><EFBFBD><EFBFBD><EFBFBD>ĵ<EFBFBD>λ<EFBFBD><CEBB>
SSX_pointPoseInfo sx_getLocationPlatePose(
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
const SSG_cornerParam cornerPara,
int* errCode)
{
*errCode = 0;
SSX_pointPoseInfo resultPose;
resultPose.center = { 0, 0, 0 };
resultPose.normalDir = { 0, 0, 0 };
int lineNum = (int)scanLines.size();
if (lineNum == 0)
{
*errCode = SG_ERR_3D_DATA_NULL;
return resultPose;
}
int linePtNum = (int)scanLines[0].size();
//<2F>ж<EFBFBD><D0B6><EFBFBD><EFBFBD>ݸ<EFBFBD>ʽ<EFBFBD>Ƿ<EFBFBD>Ϊgrid<69><64><EFBFBD>㷨ֻ<E3B7A8>ܴ<EFBFBD><DCB4><EFBFBD>grid<69><64><EFBFBD>ݸ<EFBFBD>ʽ
bool isGridData = true;
for (int line = 0; line < lineNum; line++)
{
if (linePtNum != (int)scanLines[line].size())
{
isGridData = false;
break;
}
}
if (false == isGridData)//<2F><><EFBFBD>ݲ<EFBFBD><DDB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʽ
{
*errCode = SG_ERR_NOT_GRID_FORMAT;
return resultPose;
}
//<2F><><EFBFBD><EFBFBD>ˮƽɨ<C6BD><C9A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
std::vector< std::vector<SVzNL3DPosition>> scanLines_h;
scanLines_h.resize(linePtNum);
for (int i = 0; i < linePtNum; i++)
scanLines_h[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>
scanLines_h[j][line] = scanLines[line][j];
scanLines_h[j][line].pt3D.x = scanLines[line][j].pt3D.y;
scanLines_h[j][line].pt3D.y = scanLines[line][j].pt3D.x;
}
}
for (int line = 0; line < linePtNum; line++)
{
for (int j = 0, j_max = (int)scanLines_h[line].size(); j < j_max; j++)
scanLines_h[line][j].nPointIdx = j;
}
//<2F><>ȡ<EFBFBD><C8A1>λ<EFBFBD><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˵<EFBFBD>
//<2F><>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD>
std::vector<std::vector<int>> flags;
flags.resize(lineNum);
for (int i = 0; i < lineNum; i++)
{
flags[i].resize(linePtNum);
std::fill(flags[i].begin(), flags[i].end(), 0);
}
std::vector< SVzNL3DPosition> endingPts;
for (int line = 0; line < lineNum; line++)
{
std::vector< SVzNL3DPosition> vldPts;
std::vector<SSG_RUN_EX> segs;
std::vector<int> backIndexing;
backIndexing.resize(scanLines[line].size());
wd_lineDataSegment_zDist(
scanLines[line],
vldPts,
segs,
backIndexing,
cornerPara
);
for (int i = 0; i < (int)segs.size(); i++)
{
int idx_0 = segs[i].start;
int idx_1 = segs[i].start + segs[i].len - 1;
SVzNL3DPosition pt_0 = vldPts[idx_0];
SVzNL3DPosition pt_1 = vldPts[idx_1];
flags[line][pt_0.nPointIdx] = 1;
flags[line][pt_1.nPointIdx] = 1;
pt_0.nPointIdx = pt_0.nPointIdx & 0xffff | (line << 16);
pt_1.nPointIdx = pt_1.nPointIdx & 0xffff | (line << 16);
endingPts.push_back(pt_0);
endingPts.push_back(pt_1);
}
}
for (int line = 0; line < linePtNum; line++)
{
std::vector< SVzNL3DPosition> vldPts;
std::vector<SSG_RUN_EX> segs;
std::vector<int> backIndexing;
backIndexing.resize(scanLines_h[line].size());
wd_lineDataSegment_zDist(
scanLines_h[line],
vldPts,
segs,
backIndexing,
cornerPara
);
for (int i = 0; i < (int)segs.size(); i++)
{
int idx_0 = segs[i].start;
int idx_1 = segs[i].start + segs[i].len - 1;
SVzNL3DPosition pt_0 = vldPts[idx_0];
SVzNL3DPosition pt_1 = vldPts[idx_1];
if (0 == flags[pt_0.nPointIdx][line])//<2F><><EFBFBD>ʹ<EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ<EFBFBD>ĵ<EFBFBD><C4B5>ظ<EFBFBD>
{
flags[pt_0.nPointIdx][line] = 1;
pt_0.pt3D = scanLines[pt_0.nPointIdx][line].pt3D; //ȡԭʼ<D4AD><CABC>
pt_0.nPointIdx = (pt_0.nPointIdx <<16) | (line & 0xffff);
endingPts.push_back(pt_0);
}
if (0 == flags[pt_1.nPointIdx][line]) //<2F><><EFBFBD>ʹ<EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ<EFBFBD>ĵ<EFBFBD><C4B5>ظ<EFBFBD>
{
flags[pt_1.nPointIdx][line] = 1;
pt_1.pt3D = scanLines[pt_1.nPointIdx][line].pt3D; //ȡԭʼ<D4AD><CABC>
pt_1.nPointIdx = (pt_1.nPointIdx << 16) | (line & 0xffff);
endingPts.push_back(pt_1);
}
}
}
//<2F><>ע
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>
}
for (int i = 0; i < (int)endingPts.size(); i++)
{
int line = endingPts[i].nPointIdx >> 16;
int ptIdx = endingPts[i].nPointIdx & 0x0000FFFF;
scanLines[line][ptIdx].nPointIdx = 1;
}
//<2F><><EFBFBD><EFBFBD>
//<2F>ڲ<EFBFBD><DAB2><EFBFBD><EFBFBD><EFBFBD>
int clusterCheckWin = 5;
double clusterDist = 5.0;
double topLayerThickness = 10.0;
double centerZ_R = 20.0;
int distType = 1; //0 - 2d distance; 1- 3d distance
std::vector<std::vector< SVzNL3DPosition>> objClusters; //result
wd_pointClustering_speedUp(
endingPts,
lineNum, linePtNum, clusterCheckWin, //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
clusterDist,
distType,
objClusters //result
);
//<2F>ж<EFBFBD><D0B6>ϱ<EFBFBD><CFB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
std::vector<double> objMeanZ;
objMeanZ.resize(objClusters.size());
std::vector< SSG_ROIRectD> objROIs;
objROIs.resize(objClusters.size());
//<2F><><EFBFBD>˳<EFBFBD><CBB3><EFBFBD>С<EFBFBD><D0A1>Χ<EFBFBD><CEA7>Z
double minMeanZ = -1;
for (int i = 0; i < (int)objClusters.size(); i++)
{
SSG_ROIRectD a_roi = _getListROI(objClusters[i]);
objROIs[i] = a_roi;
double w = a_roi.right - a_roi.left;
double h = a_roi.bottom - a_roi.top;
double meanZ = _getListMeanZ(objClusters[i]);
objMeanZ[i] = meanZ;
if ( (meanZ > 1e-4) && (w > 150) && (h > 100))
{
if (minMeanZ < 0)
minMeanZ = meanZ;
else
minMeanZ = minMeanZ > meanZ ? meanZ : minMeanZ;
}
}
//ѡ<><D1A1>ROI<4F><49><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
int objIdx = -1;
double maxSize = 0;
for (int i = 0; i < (int)objClusters.size(); i++)
{
if (objMeanZ[i] > 1e-4)
{
double zDist = objMeanZ[i] - minMeanZ;
if (zDist < topLayerThickness) //ͬ<>ڶ<EFBFBD><DAB6><EFBFBD>
{
double size = (objROIs[i].right - objROIs[i].left) * (objROIs[i].bottom - objROIs[i].top);
if (maxSize < -1e-4)
{
maxSize = size;
objIdx = i;
}
else
{
if (maxSize < size)
{
maxSize = size;
objIdx = i;
}
}
}
}
}
if (objIdx < 0)
{
*errCode = SG_ERR_ZERO_OBJECTS;
return resultPose;
}
//<2F><>ע
for (int i = 0; i < (int)objClusters[objIdx].size(); i++)
{
int line = objClusters[objIdx][i].nPointIdx >> 16;
int ptIdx = objClusters[objIdx][i].nPointIdx & 0x0000FFFF;
scanLines[line][ptIdx].nPointIdx = 2;
}
//<2F><><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD>
std::vector<cv::Point3f> Points3ds;
for (int i = 0; i < (int)objClusters[objIdx].size(); i++)
{
cv::Point3f a_pt = cv::Point3f(objClusters[objIdx][i].pt3D.x, objClusters[objIdx][i].pt3D.y, objClusters[objIdx][i].pt3D.z);
Points3ds.push_back(a_pt);
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>: z = Ax + By + C
//res: [0]=A, [1]= B, [2]=-1.0, [3]=C,
//std::vector<cv::Point3f> out_inliers;
//Plane res = ransacFitPlane( Points3ds, out_inliers );
Plane res = robustFitPlane(Points3ds);
//std::vector<double> res;
//vzCaculateLaserPlane(Points3ds, res);
//<2F><><EFBFBD><EFBFBD>ͶӰ<CDB6><D3B0><EFBFBD><EFBFBD>
SVzNL3DPoint vec_1 = {res.A, res.B, res.C};
SVzNL3DPoint vec_2 = { 0, 0, 1.0 };
SSG_planeCalibPara poseR = wd_computeRTMatrix(vec_1, vec_2);
//ͶӰ
std::vector<cv::Point3f> projectPoints3ds;
projectPoints3ds.resize(Points3ds.size());
double sum_x = 0, sum_y = 0;
for (int i = 0; i < (int)Points3ds.size(); i++)
{
double x = Points3ds[i].x * poseR.planeCalib[0] + Points3ds[i].y * poseR.planeCalib[1] + Points3ds[i].z * poseR.planeCalib[2];
double y = Points3ds[i].x * poseR.planeCalib[3] + Points3ds[i].y * poseR.planeCalib[4] + Points3ds[i].z * poseR.planeCalib[5];
double z = Points3ds[i].x * poseR.planeCalib[6] + Points3ds[i].y * poseR.planeCalib[7] + Points3ds[i].z * poseR.planeCalib[8];
projectPoints3ds[i] = cv::Point3f(x, y, z);
sum_x += x;
sum_y += y;
}
for (int i = 0; i < lineNum; i++)
{
if (i == 14)
int kkk = 1;
//<2F>д<EFBFBD><D0B4><EFBFBD>
//<2F><>ƽ<EFBFBD><C6BD>ȥ<EFBFBD><C8A5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
lineDataRT_vector(scanLines[i], poseR.planeCalib, -1);
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
double center_x = sum_x / (double)Points3ds.size();
double center_y = sum_y / (double)Points3ds.size();
//<2F><><EFBFBD><EFBFBD>4<EFBFBD><34><EFBFBD><EFBFBD>
SVzNL2DPointD top_pt = { center_x, center_y - centerZ_R };
SVzNL2DPointD bottom_pt = { center_x, center_y + centerZ_R };
SVzNL2DPointD left_pt = { center_x - centerZ_R, center_y};
SVzNL2DPointD roght_pt = { center_x + centerZ_R, center_y};
//<2F><><EFBFBD><EFBFBD>Z
SVzNL3DPoint ptTop = { 0, 0, 0 };
SVzNL3DPoint ptBtm = { 0, 0, 0 };
SVzNL3DPoint ptLeft = { 0, 0, 0 };
SVzNL3DPoint ptRight = { 0, 0, 0 };
double minDistTop = -1, minDistBtm = -1, minDistLeft = -1, minDistRight = -1;
for (int line = 0; line < lineNum; line++)
{
for (int j = 0; j < linePtNum; j++)
{
if (scanLines[line][j].pt3D.z > 1e-4)
{
//top
if (minDistTop < 1e-4)
{
ptTop = scanLines[line][j].pt3D;
minDistTop = sqrt(pow(top_pt.x - scanLines[line][j].pt3D.x, 2) + pow(top_pt.y - scanLines[line][j].pt3D.y, 2));
}
else
{
double dist = sqrt(pow(top_pt.x - scanLines[line][j].pt3D.x, 2) + pow(top_pt.y - scanLines[line][j].pt3D.y, 2));
if (minDistTop > dist)
{
ptTop = scanLines[line][j].pt3D;
minDistTop = dist;
}
}
//bottom
if (minDistBtm < 1e-4)
{
ptBtm = scanLines[line][j].pt3D;
minDistBtm = sqrt(pow(bottom_pt.x - scanLines[line][j].pt3D.x, 2) + pow(bottom_pt.y - scanLines[line][j].pt3D.y, 2));
}
else
{
double dist = sqrt(pow(bottom_pt.x - scanLines[line][j].pt3D.x, 2) + pow(bottom_pt.y - scanLines[line][j].pt3D.y, 2));
if (minDistBtm > dist)
{
ptBtm = scanLines[line][j].pt3D;
minDistBtm = dist;
}
}
//left
if (minDistLeft < 1e-4)
{
ptLeft = scanLines[line][j].pt3D;
minDistLeft = sqrt(pow(left_pt.x - scanLines[line][j].pt3D.x, 2) + pow(left_pt.y - scanLines[line][j].pt3D.y, 2));
}
else
{
double dist = sqrt(pow(left_pt.x - scanLines[line][j].pt3D.x, 2) + pow(left_pt.y - scanLines[line][j].pt3D.y, 2));
if (minDistLeft > dist)
{
ptLeft = scanLines[line][j].pt3D;
minDistLeft = dist;
}
}
//right
if (minDistRight < 1e-4)
{
ptRight = scanLines[line][j].pt3D;
minDistRight = sqrt(pow(roght_pt.x - scanLines[line][j].pt3D.x, 2) + pow(roght_pt.y - scanLines[line][j].pt3D.y, 2));
}
else
{
double dist = sqrt(pow(roght_pt.x - scanLines[line][j].pt3D.x, 2) + pow(roght_pt.y - scanLines[line][j].pt3D.y, 2));
if (minDistRight > dist)
{
ptRight = scanLines[line][j].pt3D;
minDistRight = dist;
}
}
}
}
}
double center_z = (ptTop.z + ptBtm.z + ptLeft.z + ptRight.z) / 4;
resultPose.center = { center_x, center_y, center_z };
resultPose.normalDir = { 0, 0, -1.0 };
//<2F><>ת<EFBFBD><D7AA>ȥ
for (int i = 0; i < lineNum; i++)
{
//<2F>д<EFBFBD><D0B4><EFBFBD>
//<2F><>ƽ<EFBFBD><C6BD>ȥ<EFBFBD><C8A5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
lineDataRT_vector(scanLines[i], poseR.invRMatrix, -1);
}
double x = resultPose.center.x * poseR.invRMatrix[0] + resultPose.center.y * poseR.invRMatrix[1] + resultPose.center.z * poseR.invRMatrix[2];
double y = resultPose.center.x * poseR.invRMatrix[3] + resultPose.center.y * poseR.invRMatrix[4] + resultPose.center.z * poseR.invRMatrix[5];
double z = resultPose.center.x * poseR.invRMatrix[6] + resultPose.center.y * poseR.invRMatrix[7] + resultPose.center.z * poseR.invRMatrix[8];
resultPose.center = { x, y, z };
x = resultPose.normalDir.x * poseR.invRMatrix[0] + resultPose.normalDir.y * poseR.invRMatrix[1] + resultPose.normalDir.z * poseR.invRMatrix[2];
y = resultPose.normalDir.x * poseR.invRMatrix[3] + resultPose.normalDir.y * poseR.invRMatrix[4] + resultPose.normalDir.z * poseR.invRMatrix[5];
z = resultPose.normalDir.x * poseR.invRMatrix[6] + resultPose.normalDir.y * poseR.invRMatrix[7] + resultPose.normalDir.z * poseR.invRMatrix[8];
resultPose.normalDir = { x, y, z };
return resultPose;
}
rodAndBarDetection Initial check in
2026-01-11 17:04:06 +08:00
rodAarcFeatueDetection version 1.1.0 : 添加了地面调平和棒材定位
2026-03-21 23:15:43 +08:00
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];
//<2F>˲<EFBFBD><CBB2><EFBFBD><EFBFBD>˳<EFBFBD><CBB3>쳣<EFBFBD><ECB3A3>
sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam);
//<2F><>ȡrodArc<72><63><EFBFBD><EFBFBD>
std::vector<SWD_rodArcFeature> line_rodArcs;
wd_getRodArcFeature_peakCornerMethod(lineData, line, rodDiameter/2, cornerPara, line_rodArcs );
arcFeatures.push_back(line_rodArcs);
}
return;
}
rodAarcFeatueDetection version 1.1.1 : 初始发布给客户的版本
2026-03-22 09:49:06 +08:00
SVzNL3DPoint _exchangeXY(SVzNL3DPoint pt)
{
SVzNL3DPoint result = {pt.y, pt.x, pt.z};
return result;
}
void _computeRodInfo(
SWD_rodArcFeatureTree& a_objTree,
bool treeIsHorizon,
std::vector< SVzNL3DPoint>& fittingPoints,
SSX_rodPositionInfo& a_objRod)
{
int nodeSize = a_objTree.treeNodes.size();
//<2F><><EFBFBD><EFBFBD>
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);
double deltaZ = fittingPoints[dataSize - 1].z - fittingPoints[0].z;
double len = sqrt(pow(foot1.x - foot2.x, 2) + pow(foot1.y - foot2.y, 2));
//ֱ<>ߵ<EFBFBD><DFB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
SVzNL3DPoint axialDir = { foot2.x - foot1.x, foot2.y - foot1.y, fittingPoints[dataSize - 1].z - fittingPoints[0].z };
//<2F><>һ<EFBFBD><D2BB>
double normData = sqrt(pow(axialDir.x, 2) + pow(axialDir.y, 2) + pow(axialDir.z, 2));
axialDir.x = axialDir.x / normData;
axialDir.y = axialDir.y / normData;
axialDir.z = axialDir.z / normData;
//<2F><><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƽ<EFBFBD>棨Y=0ƽ<30><C6BD><EFBFBD><EFBFBD>תһ<D7AA><D2BB><EFBFBD>Ƕȣ<C7B6><C8A3>ķ<EFBFBD><C4B7><EFBFBD>
double theta = atan2(foot2.y - foot1.y, foot2.x - foot1.x);
double sinTheta = sin(theta);
double cosTheta = cos(theta);
SVzNL3DPoint tmpDir = { sinTheta, -cosTheta, 0 };
//<2F><><EFBFBD>˳<EFBFBD><CBB3><EFBFBD><EFBFBD>ĵķ<C4B5><C4B7><EFBFBD>
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 };
//ȷ<><C8B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>յ㣺<D5B5><E3A3BA>ֱ<EFBFBD>߾<EFBFBD><DFBE><EFBFBD>С<EFBFBD><D0A1>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++)
{
SVzNL3DPoint a_pt = a_objTree.treeNodes[j].peakPt;
if (true == treeIsHorizon)
a_pt = _exchangeXY(a_pt);
double dist = abs(a_pt.x * _a + a_pt.y * _b + _c);
if (dist < 5.0)
{
realStart = a_pt;
foundStart = true;
break;
}
}
if (false == foundStart)
{
realStart = a_objTree.treeNodes[0].peakPt;
if (true == treeIsHorizon)
realStart = _exchangeXY(realStart);
}
bool foundEnd = false;
for (int j = nodeSize - 1; j >= 0; j--)
{
SVzNL3DPoint a_pt = a_objTree.treeNodes[j].peakPt;
if (true == treeIsHorizon)
a_pt = _exchangeXY(a_pt);
double dist = abs(a_pt.x * _a + a_pt.y * _b + _c);
if (dist < 5.0)
{
realEnd = a_pt;
foundEnd = true;
break;
}
}
if (false == foundEnd)
{
realEnd = a_objTree.treeNodes[nodeSize - 1].peakPt;
if (true == treeIsHorizon)
realEnd = _exchangeXY(realEnd);
}
SVzNL2DPointD foot_s = sx_getFootPoint_abc(realStart.x, realStart.y, _a, _b, _c);
SVzNL2DPointD foot_e = sx_getFootPoint_abc(realEnd.x, realEnd.y, _a, _b, _c);
double dist_s = sqrt(pow(foot_s.x - foot1.x, 2) + pow(foot_s.y - foot1.y, 2));
double dist_e = sqrt(pow(foot_e.x - foot1.x, 2) + pow(foot_e.y - foot1.y, 2));
//<2F><><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD>Ϣ
;
a_objRod.startPt = { foot_s.x, foot_s.y, -(dist_s / len) * deltaZ + fittingPoints[0].z };
a_objRod.endPt = { foot_e.x, foot_e.y, (dist_e / len) * deltaZ + fittingPoints[0].z };
a_objRod.center = { (a_objRod.startPt.x + a_objRod.endPt.x) / 2,
(a_objRod.startPt.y + a_objRod.endPt.y) / 2,
(a_objRod.startPt.z + a_objRod.endPt.z) / 2 };
a_objRod.axialDir = axialDir;
a_objRod.normalDir = normalDir;
return;
}
bool checkObjEixst(SSX_rodPositionInfo& a_objRod, std::vector<SSX_rodPositionInfo>& existObjs, const SSX_rodParam rodParam)
{
double minDistance = -1;
for (int i = 0; i < (int)existObjs.size(); i++)
{
double dist = sqrt(pow(a_objRod.center.x - existObjs[i].center.x, 2) +
pow(a_objRod.center.y - existObjs[i].center.y, 2) +
pow(a_objRod.center.z - existObjs[i].center.z, 2));
if (minDistance < 0)
minDistance = dist;
else if (minDistance > dist)
minDistance = dist;
}
if (minDistance < 0)
return false;
else if (minDistance < rodParam.diameter / 4) // R/2
return true;
else
return false;
}
bool _commpareByCenterZ(SSX_rodPositionInfo& a, SSX_rodPositionInfo& b)
{
return (a.center.z < b.center.z);
}
rodAarcFeatueDetection version 1.1.0 : 添加了地面调平和棒材定位
2026-03-21 23:15:43 +08:00
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();
//<2F>ж<EFBFBD><D0B6><EFBFBD><EFBFBD>ݸ<EFBFBD>ʽ<EFBFBD>Ƿ<EFBFBD>Ϊgrid<69><64><EFBFBD>㷨ֻ<E3B7A8>ܴ<EFBFBD><DCB4><EFBFBD>grid<69><64><EFBFBD>ݸ<EFBFBD>ʽ
bool isGridData = true;
for (int line = 0; line < lineNum; line++)
{
if (linePtNum != (int)scanLines[line].size())
{
isGridData = false;
break;
}
}
if (false == isGridData)//<2F><><EFBFBD>ݲ<EFBFBD><DDB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʽ
{
*errCode = SG_ERR_NOT_GRID_FORMAT;
return;
}
rodAndBarDetection Initial check in
2026-01-11 17:04:06 +08:00
rodAarcFeatueDetection version 1.1.0 : 添加了地面调平和棒材定位
2026-03-21 23:15:43 +08:00
for (int i = 0, i_max = (int)scanLines.size(); i < i_max; i++)
{
if (i == 14)
int kkk = 1;
//<2F>д<EFBFBD><D0B4><EFBFBD>
//<2F><>ƽ<EFBFBD><C6BD>ȥ<EFBFBD><C8A5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
double cuttingZ = -1;
sx_rodPosition_lineDataR(scanLines[i], poseCalibPara.planeCalib, cuttingZ);
}
//<2F><><EFBFBD><EFBFBD>ˮƽɨ<C6BD><C9A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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;
}
}
//<2F>ڴ<EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD>Ϸֱ<CFB7><D6B1><EFBFBD>ȡARC<52><43><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
std::vector<std::vector<SWD_rodArcFeature>> arcFeatures_v;
rodAarcFeatueDetection( scanLines, cornerPara, filterParam, rodParam.diameter, arcFeatures_v);
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
std::vector<SWD_rodArcFeatureTree> rodArcTrees_v;
wd_getRodArcFeatureGrowingTrees(arcFeatures_v, rodArcTrees_v, growParam);
//ˮƽ<CBAE><C6BD><EFBFBD><EFBFBD>
std::vector<std::vector<SWD_rodArcFeature>> arcFeatures_h;
rodAarcFeatueDetection(hLines_raw, cornerPara, filterParam, rodParam.diameter, arcFeatures_h);
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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; //<2F><>ԭʼ<D4AD><CABC><EFBFBD>ݵ<EFBFBD><DDB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0<EFBFBD><30><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>ʹ<EFBFBD>ã<EFBFBD>
}
}
//<2F>ñ<EFBFBD>־<EFBFBD><D6BE><EFBFBD><EFBFBD><EFBFBD><EFBFBD>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;
}
}
//<2F>ñ<EFBFBD>־<EFBFBD><D6BE><EFBFBD><EFBFBD><EFBFBD><EFBFBD>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;
}
}
//Ŀ<><C4BF><EFBFBD>ж<EFBFBD>
//(1)<29><><EFBFBD>ȹ<EFBFBD><C8B9><EFBFBD>
rodAarcFeatueDetection version 1.1.1 : 初始发布给客户的版本
2026-03-22 09:49:06 +08:00
//<2F><>ֱĿ<D6B1><C4BF>
rodAarcFeatueDetection version 1.1.0 : 添加了地面调平和棒材定位
2026-03-21 23:15:43 +08:00
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);
rodAarcFeatueDetection version 1.1.1 : 初始发布给客户的版本
2026-03-22 09:49:06 +08:00
if (lenDiff < rodParam.len* 0.15) //validObj
rodAarcFeatueDetection version 1.1.0 : 添加了地面调平和棒材定位
2026-03-21 23:15:43 +08:00
{
//<2F><>XYƽ<59><C6BD><EFBFBD><EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//Ϊ<>˷<EFBFBD>ֹ<EFBFBD>˲<EFBFBD>Ӱ<EFBFBD>죬<EFBFBD><ECA3AC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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;
SSX_rodPositionInfo a_objRod;
rodAarcFeatueDetection version 1.1.1 : 初始发布给客户的版本
2026-03-22 09:49:06 +08:00
_computeRodInfo(rodArcTrees_v[i], false, fittingPoints, a_objRod);
rodInfo.push_back(a_objRod);
}
}
//ˮƽĿ<C6BD><C4BF>
for (int i = 0; i < (int)rodArcTrees_h.size(); i++)
{
int nodeSize = rodArcTrees_h[i].treeNodes.size();
SVzNL3DPoint startCenter = rodArcTrees_h[i].treeNodes[0].peakPt;
startCenter = _exchangeXY(startCenter);
SVzNL3DPoint endCenter = rodArcTrees_h[i].treeNodes[nodeSize - 1].peakPt;
endCenter = _exchangeXY(endCenter);
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 < rodParam.len * 0.15) //validObj
{
//<2F><>XYƽ<59><C6BD><EFBFBD><EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//Ϊ<>˷<EFBFBD>ֹ<EFBFBD>˲<EFBFBD>Ӱ<EFBFBD>죬<EFBFBD><ECA3AC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
std::vector<SVzNL3DPoint> fittingPoints;
rodAarcFeatueDetection version 1.1.0 : 添加了地面调平和棒材定位
2026-03-21 23:15:43 +08:00
for (int j = 0; j < nodeSize; j++)
{
rodAarcFeatueDetection version 1.1.1 : 初始发布给客户的版本
2026-03-22 09:49:06 +08:00
SVzNL3DPoint a_pt = _exchangeXY(rodArcTrees_h[i].treeNodes[j].peakPt);
double dist1 = sqrt(pow(a_pt.x - startCenter.x, 2) + pow(a_pt.y - startCenter.y, 2) + pow(a_pt.z - startCenter.z, 2));
double dist2 = sqrt(pow(a_pt.x - endCenter.x, 2) + pow(a_pt.y - endCenter.y, 2) + pow(a_pt.z - endCenter.z, 2));
if ((dist1 > rodParam.diameter / 2) && (dist2 > rodParam.diameter / 2))
fittingPoints.push_back(a_pt);
rodAarcFeatueDetection version 1.1.0 : 添加了地面调平和棒材定位
2026-03-21 23:15:43 +08:00
}
rodAarcFeatueDetection version 1.1.1 : 初始发布给客户的版本
2026-03-22 09:49:06 +08:00
if (fittingPoints.size() < 3)
continue;
rodAarcFeatueDetection version 1.1.0 : 添加了地面调平和棒材定位
2026-03-21 23:15:43 +08:00
rodAarcFeatueDetection version 1.1.1 : 初始发布给客户的版本
2026-03-22 09:49:06 +08:00
SSX_rodPositionInfo a_objRod;
_computeRodInfo(rodArcTrees_h[i], true, fittingPoints, a_objRod);
//<2F><><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD><C7B7>봹ֱ<EBB4B9><D6B1><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD>ص<EFBFBD>
bool isExist = checkObjEixst(a_objRod, rodInfo, rodParam);
if(false == isExist)
rodInfo.push_back(a_objRod);
rodAarcFeatueDetection version 1.1.0 : 添加了地面调平和棒材定位
2026-03-21 23:15:43 +08:00
}
}
//<2F><>2<EFBFBD><32><EFBFBD>ڵ<EFBFBD><DAB5>ж<EFBFBD>
rodAarcFeatueDetection version 1.1.1 : 初始发布给客户的版本
2026-03-22 09:49:06 +08:00
//<2F><><EFBFBD>߶<EFBFBD><DFB6><EFBFBD><EFBFBD><EFBFBD>
std::sort(rodInfo.begin(), rodInfo.end(), _commpareByCenterZ);
rodAarcFeatueDetection version 1.1.0 : 添加了地面调平和棒材定位
2026-03-21 23:15:43 +08:00
//<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_rodPosition_lineDataR(scanLines[i], poseCalibPara.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>ϵ
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;
rodAarcFeatueDetection version 1.1.1 : 初始发布给客户的版本
2026-03-22 09:49:06 +08:00
rawObj = _translatePoint(a_rod.startPt, poseCalibPara.invRMatrix);
a_rod.startPt = rawObj;
rawObj = _translatePoint(a_rod.endPt, poseCalibPara.invRMatrix);
a_rod.endPt = rawObj;
rodAarcFeatueDetection version 1.1.0 : 添加了地面调平和棒材定位
2026-03-21 23:15:43 +08:00
}
return;
}
rodAndBarDetection Initial check in
2026-01-11 17:04:06 +08:00
rodAndBarDetection version 1.2.0 : 配天螺杆测量增加了定位盘中心测量功能
2026-04-14 15:04:46 +08:00
//<2F><>ԣ<EFBFBD>ֽṹ<D6BD>ֽ<D6BD>춨λ
void sx_rebarWeldSeamPositioning(
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_weldSeamInfo>& weldSeamInfo,
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();
//<2F>ж<EFBFBD><D0B6><EFBFBD><EFBFBD>ݸ<EFBFBD>ʽ<EFBFBD>Ƿ<EFBFBD>Ϊgrid<69><64><EFBFBD>㷨ֻ<E3B7A8>ܴ<EFBFBD><DCB4><EFBFBD>grid<69><64><EFBFBD>ݸ<EFBFBD>ʽ
bool isGridData = true;
for (int line = 0; line < lineNum; line++)
{
if (linePtNum != (int)scanLines[line].size())
{
isGridData = false;
break;
}
}
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_max = (int)scanLines.size(); i < i_max; i++)
{
if (i == 14)
int kkk = 1;
//<2F>д<EFBFBD><D0B4><EFBFBD>
//<2F><>ƽ<EFBFBD><C6BD>ȥ<EFBFBD><C8A5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
double cuttingZ = -1;
sx_rodPosition_lineDataR(scanLines[i], poseCalibPara.planeCalib, cuttingZ);
}
//Ѱ<><D1B0>ˮƽ<CBAE>ʹ<EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD>ĸֽ<C4B8>
//<2F><><EFBFBD><EFBFBD>ˮƽɨ<C6BD><C9A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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;
}
}
//<2F>ڴ<EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD>Ϸֱ<CFB7><D6B1><EFBFBD>ȡARC<52><43><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
std::vector<std::vector<SWD_rodArcFeature>> arcFeatures_v;
rodAarcFeatueDetection(scanLines, cornerPara, filterParam, rodParam.diameter, arcFeatures_v);
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
std::vector<SWD_rodArcFeatureTree> rodArcTrees_v;
wd_getRodArcFeatureGrowingTrees(arcFeatures_v, rodArcTrees_v, growParam);
//ˮƽ<CBAE><C6BD><EFBFBD><EFBFBD>
std::vector<std::vector<SWD_rodArcFeature>> arcFeatures_h;
rodAarcFeatueDetection(hLines_raw, cornerPara, filterParam, rodParam.diameter, arcFeatures_h);
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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; //<2F><>ԭʼ<D4AD><CABC><EFBFBD>ݵ<EFBFBD><DDB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0<EFBFBD><30><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>ʹ<EFBFBD>ã<EFBFBD>
}
}
//<2F>ñ<EFBFBD>־<EFBFBD><D6BE><EFBFBD><EFBFBD><EFBFBD><EFBFBD>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;
}
}
//<2F>ñ<EFBFBD>־<EFBFBD><D6BE><EFBFBD><EFBFBD><EFBFBD><EFBFBD>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;
}
}
}
Reference in New Issue Copy Permalink