Merge branch 'main' of http://gitea.mnutil.com/jerryzeng/algoLib
This commit is contained in:
杰仔
commit
f5d8ba8af2
@ -429,13 +429,13 @@ void _outputRGBDScan_RGBD_rodInfo(
|
||||
int centerFlag = pt3D->nPointIdx >> 4;
|
||||
if (centerFlag > 0)
|
||||
{
|
||||
rgb = { 250, 0, 0 };
|
||||
size = 6;
|
||||
rgb = { 180, 0, 0 };
|
||||
size = 2;
|
||||
}
|
||||
else
|
||||
{
|
||||
rgb = objColor[pt3D->nPointIdx % 8];
|
||||
size = 3;
|
||||
size = 2;
|
||||
}
|
||||
|
||||
|
||||
@ -457,10 +457,13 @@ void _outputRGBDScan_RGBD_rodInfo(
|
||||
if (objNum > 0)
|
||||
{
|
||||
sw << "Line_" << lineIdx << "_0_" << objNum << std::endl;
|
||||
rgb = { 250, 0, 0 };
|
||||
size = 8;
|
||||
size = 12;
|
||||
for (int i = 0; i < objNum; i++)
|
||||
{
|
||||
if(i == 0)
|
||||
rgb = { 250, 255, 0 };
|
||||
else
|
||||
rgb = { 250, 0, 0 };
|
||||
float x = (float)rodInfo[i].center.x;
|
||||
float y = (float)rodInfo[i].center.y;
|
||||
float z = (float)rodInfo[i].center.z;
|
||||
@ -468,51 +471,42 @@ void _outputRGBDScan_RGBD_rodInfo(
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
|
||||
}
|
||||
//多输出一个，修正显示工具bug
|
||||
float x = (float)rodInfo[0].center.x;
|
||||
float y = (float)rodInfo[0].center.y;
|
||||
float z = (float)rodInfo[0].center.z;
|
||||
sw << "{" << x << "," << y << "," << z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
|
||||
|
||||
//输出法向
|
||||
size = 1;
|
||||
size = 8;
|
||||
double len = 60;
|
||||
lineIdx = 0;
|
||||
for (int i = 0; i < objNum; i++)
|
||||
{
|
||||
SVzNL3DPoint pt0 = { rodInfo[i].center.x - len * rodInfo[i].axialDir.x,
|
||||
rodInfo[i].center.y - len * rodInfo[i].axialDir.y,
|
||||
rodInfo[i].center.z - len * rodInfo[i].axialDir.z };
|
||||
if (i == 0)
|
||||
rgb = { 250, 255, 0 };
|
||||
else
|
||||
rgb = { 250, 0, 0 };
|
||||
SVzNL3DPoint pt0 = { rodInfo[i].center.x, rodInfo[i].center.y, rodInfo[i].center.z };
|
||||
SVzNL3DPoint pt1 = { rodInfo[i].center.x + len * rodInfo[i].axialDir.x,
|
||||
rodInfo[i].center.y + len * rodInfo[i].axialDir.y,
|
||||
rodInfo[i].center.z + len * rodInfo[i].axialDir.z };
|
||||
SVzNL3DPoint pt2 = { rodInfo[i].center.x + len * rodInfo[i].normalDir.x,
|
||||
rodInfo[i].center.y + len * rodInfo[i].normalDir.y,
|
||||
rodInfo[i].center.z + len * rodInfo[i].normalDir.z };
|
||||
//显示轴向量
|
||||
sw << "Poly_" << lineIdx << "_2" << std::endl;
|
||||
sw << "{" << (float)rodInfo[i].startPt.x << "," << (float)rodInfo[i].startPt.y << "," << (float)rodInfo[i].startPt.z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
|
||||
sw << "{" << (float)rodInfo[i].endPt.x << "," << (float)rodInfo[i].endPt.y << "," << (float)rodInfo[i].endPt.z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
|
||||
lineIdx++;
|
||||
//显示法向量
|
||||
sw << "Poly_" << lineIdx << "_2" << std::endl;
|
||||
sw << "{" << (float)pt0.x << "," << (float)pt0.y << "," << (float)pt0.z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
|
||||
sw << "{" << pt1.x << "," << pt1.y << "," << pt1.z << "}-";
|
||||
sw << "{" << (float)pt2.x << "," << (float)pt2.y << "," << (float)pt2.z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
|
||||
lineIdx++;
|
||||
}
|
||||
//多输出一个，修正显示工具bug
|
||||
SVzNL3DPoint pt0 = { rodInfo[0].center.x - len * rodInfo[0].axialDir.x,
|
||||
rodInfo[0].center.y - len * rodInfo[0].axialDir.y,
|
||||
rodInfo[0].center.z - len * rodInfo[0].axialDir.z };
|
||||
SVzNL3DPoint pt1 = { rodInfo[0].center.x + len * rodInfo[0].axialDir.x,
|
||||
rodInfo[0].center.y + len * rodInfo[0].axialDir.y,
|
||||
rodInfo[0].center.z + len * rodInfo[0].axialDir.z };
|
||||
//显示法向量
|
||||
sw << "Poly_" << lineIdx << "_2" << std::endl;
|
||||
sw << "{" << (float)pt0.x << "," << (float)pt0.y << "," << (float)pt0.z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
|
||||
sw << "{" << pt1.x << "," << pt1.y << "," << pt1.z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
|
||||
lineIdx++;
|
||||
}
|
||||
sw.close();
|
||||
@ -630,7 +624,7 @@ void rodPositionTest(void)
|
||||
|
||||
for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
|
||||
{
|
||||
fidx =1;
|
||||
//fidx =1;
|
||||
char _scan_file[256];
|
||||
sprintf_s(_scan_file, "%sLaserData_%d.txt", dataPath[grp], fidx);
|
||||
|
||||
|
||||
@ -7,7 +7,8 @@
|
||||
|
||||
//version 1.0.0 : base version release to customer
|
||||
//version 1.1.0 : 添加了地面调平和棒材定位
|
||||
std::string m_strVersion = "1.1.0";
|
||||
//version 1.1.1 : 初始发布给客户的版本
|
||||
std::string m_strVersion = "1.1.1";
|
||||
const char* wd_rodAndBarDetectionVersion(void)
|
||||
{
|
||||
return m_strVersion.c_str();
|
||||
@ -477,6 +478,136 @@ void rodAarcFeatueDetection(
|
||||
return;
|
||||
}
|
||||
|
||||
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();
|
||||
//拟合
|
||||
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));
|
||||
//直线的轴向向量
|
||||
SVzNL3DPoint axialDir = { foot2.x - foot1.x, foot2.y - foot1.y, fittingPoints[dataSize - 1].z - fittingPoints[0].z };
|
||||
//归一化
|
||||
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;
|
||||
//计算一个辅助平面（Y=0平面旋转一个角度）的法向
|
||||
double theta = atan2(foot2.y - foot1.y, foot2.x - foot1.x);
|
||||
double sinTheta = sin(theta);
|
||||
double cosTheta = cos(theta);
|
||||
SVzNL3DPoint tmpDir = { sinTheta, -cosTheta, 0 };
|
||||
//叉乘出棒材的法向
|
||||
SVzNL3DPoint normalDir = { axialDir.y * tmpDir.z - tmpDir.y * axialDir.z,
|
||||
axialDir.z * tmpDir.x - tmpDir.z * axialDir.x,
|
||||
axialDir.x * tmpDir.y - tmpDir.x * axialDir.y };
|
||||
|
||||
|
||||
//确定真正起点和终点：到直线距离小于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));
|
||||
//生成目标信息
|
||||
;
|
||||
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);
|
||||
}
|
||||
|
||||
void sx_rodPositioning(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||||
const SSG_planeCalibPara poseCalibPara,
|
||||
@ -597,6 +728,7 @@ void sx_rodPositioning(
|
||||
}
|
||||
//目标判断
|
||||
//(1)长度过滤
|
||||
//垂直目标
|
||||
for (int i = 0; i < (int)rodArcTrees_v.size(); i++)
|
||||
{
|
||||
int nodeSize = rodArcTrees_v[i].treeNodes.size();
|
||||
@ -606,7 +738,7 @@ void sx_rodPositioning(
|
||||
pow(startCenter.y - endCenter.y, 2) +
|
||||
pow(startCenter.z - endCenter.z, 2));
|
||||
double lenDiff = abs(len - rodParam.len);
|
||||
if (lenDiff < 10.0) //validObj
|
||||
if (lenDiff < rodParam.len* 0.15) //validObj
|
||||
{
|
||||
//在XY平面内直线拟合
|
||||
//为了防止端部影响，跳过端面数据
|
||||
@ -624,48 +756,51 @@ void sx_rodPositioning(
|
||||
}
|
||||
if (fittingPoints.size() < 3)
|
||||
continue;
|
||||
//拟合
|
||||
double _a, _b, _c;
|
||||
lineFitting_abc(fittingPoints, &_a, &_b, &_c);
|
||||
int dataSize = (int)fittingPoints.size();
|
||||
SVzNL2DPointD foot1 = sx_getFootPoint_abc(fittingPoints[0].x, fittingPoints[0].y, _a, _b, _c);
|
||||
SVzNL2DPointD foot2 = sx_getFootPoint_abc(fittingPoints[dataSize - 1].x, fittingPoints[dataSize - 1].y, _a, _b, _c);
|
||||
//直线的轴向向量
|
||||
SVzNL3DPoint axialDir = { foot2.x - foot1.x, foot2.y - foot1.y, fittingPoints[dataSize - 1].z - fittingPoints[0].z };
|
||||
double theta = atan2(foot2.y - foot1.y, foot2.x - foot1.x);
|
||||
double sinTheta = sin(theta);
|
||||
double cosTheta = cos(theta);
|
||||
SVzNL3DPoint tmpDir = { sinTheta, -cosTheta, 0 };
|
||||
SVzNL3DPoint normalDir = { axialDir.y * tmpDir.z - tmpDir.y * axialDir.z,
|
||||
axialDir.z * tmpDir.x - tmpDir.z * axialDir.x,
|
||||
axialDir.x * tmpDir.y - tmpDir.x * axialDir.y };
|
||||
|
||||
SSX_rodPositionInfo a_objRod;
|
||||
//确定真正起点和终点：到直线距离小于5mm
|
||||
double tmpData = sqrt(_a * _a + _b * _b);
|
||||
_a = _a / tmpData;
|
||||
_b = _b / tmpData;
|
||||
_c = _c / tmpData;
|
||||
SVzNL3DPoint realStart, realEnd;
|
||||
bool foundStart = false;
|
||||
_computeRodInfo(rodArcTrees_v[i], false, fittingPoints, a_objRod);
|
||||
rodInfo.push_back(a_objRod);
|
||||
}
|
||||
}
|
||||
//水平目标
|
||||
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
|
||||
{
|
||||
//在XY平面内直线拟合
|
||||
//为了防止端部影响，跳过端面数据
|
||||
std::vector<SVzNL3DPoint> fittingPoints;
|
||||
for (int j = 0; j < nodeSize; j++)
|
||||
{
|
||||
double dist = abs(rodArcTrees_v[i].treeNodes[j].peakPt.x * _a +
|
||||
rodArcTrees_v[i].treeNodes[j].peakPt.y * _b + _c);
|
||||
if (dist < 5.0)
|
||||
{
|
||||
realStart = rodArcTrees_v[i].treeNodes[j].peakPt;
|
||||
foundStart = true;
|
||||
break;
|
||||
}
|
||||
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);
|
||||
}
|
||||
if (fittingPoints.size() < 3)
|
||||
continue;
|
||||
|
||||
|
||||
SSX_rodPositionInfo a_objRod;
|
||||
_computeRodInfo(rodArcTrees_h[i], true, fittingPoints, a_objRod);
|
||||
//检查是否与垂直检测目标重叠
|
||||
bool isExist = checkObjEixst(a_objRod, rodInfo, rodParam);
|
||||
if(false == isExist)
|
||||
rodInfo.push_back(a_objRod);
|
||||
}
|
||||
}
|
||||
//（2）遮挡判断
|
||||
|
||||
|
||||
//按高度排序
|
||||
std::sort(rodInfo.begin(), rodInfo.end(), _commpareByCenterZ);
|
||||
|
||||
//将数据重新投射回原来的坐标系，以保持手眼标定结果正确
|
||||
for (int i = 0; i < lineNum; i++)
|
||||
@ -680,6 +815,11 @@ void sx_rodPositioning(
|
||||
a_rod.axialDir = rawObj;
|
||||
rawObj = _translatePoint(a_rod.normalDir, poseCalibPara.invRMatrix);
|
||||
a_rod.normalDir = rawObj;
|
||||
|
||||
rawObj = _translatePoint(a_rod.startPt, poseCalibPara.invRMatrix);
|
||||
a_rod.startPt = rawObj;
|
||||
rawObj = _translatePoint(a_rod.endPt, poseCalibPara.invRMatrix);
|
||||
a_rod.endPt = rawObj;
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user