workpieceHolePositioning v1.0.1
初始发布版本。
This commit is contained in:
parent
a70fd41084
commit
b7196a12ea
@ -6,7 +6,7 @@
|
||||
#include <limits>
|
||||
|
||||
//version 1.0.0 : base version release to customer
|
||||
std::string m_strVersion = "1.0.0";
|
||||
std::string m_strVersion = "1.0.1";
|
||||
const char* wd_workpieceHolePositioningVersion(void)
|
||||
{
|
||||
return m_strVersion.c_str();
|
||||
@ -117,6 +117,38 @@ int angleConditionDistanceSearch(
|
||||
return result;
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
//工件孔定位
|
||||
void wd_workpieceHolePositioning(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLinesInput,
|
||||
@ -155,8 +187,48 @@ void wd_workpieceHolePositioning(
|
||||
}
|
||||
|
||||
//生成量化数据，以1mm为量化尺度，用于确定工件表面高度
|
||||
|
||||
|
||||
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);//初始化为0
|
||||
quantiHist[i].resize(quanti_Y);
|
||||
std::fill(quantiHist[i].begin(), quantiHist[i].end(), 0);//初始化为0
|
||||
}
|
||||
//以1mm尺度量化
|
||||
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];
|
||||
}
|
||||
}
|
||||
|
||||
std::vector<std::vector<int>> pointMask;
|
||||
pointMask.resize(lineNum);
|
||||
@ -402,24 +474,46 @@ void wd_workpieceHolePositioning(
|
||||
p3.radius = -1;
|
||||
|
||||
//重新计算Z值。因为沉孔的原因，Z值会不准确。取四条边的中点处的Z值的均值作为整个的Z值
|
||||
|
||||
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;
|
||||
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);
|
||||
|
||||
for (int m = 0; m < 4; m++)
|
||||
{
|
||||
SVzNL3DPoint a_pt = a_workpiece.holes[m];
|
||||
a_pt.z = a_pt.z + 20; //法向，因为做过地面高平，所以法向只在z向
|
||||
a_workpiece.holesDir.push_back(a_pt);
|
||||
}
|
||||
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,
|
||||
(p0.center.z + p1.center.z + p2.center.z + p3.center.z) / 4 };
|
||||
(z1 + z2 + z3 + z4) / 4 };
|
||||
|
||||
SVzNL3DPoint y_dir;
|
||||
if (p0.center.y < p2.center.y)
|
||||
y_dir = { p0.center.x - p2.center.x, p0.center.y - p2.center.y, a_workpiece.center.z };
|
||||
y_dir = { p0.center.x - p2.center.x, p0.center.y - p2.center.y, 0 };
|
||||
else
|
||||
y_dir = { p2.center.x - p0.center.x, p2.center.y - p0.center.y, a_workpiece.center.z };
|
||||
a_workpiece.y_dir = { y_dir.x + a_workpiece.center.x, y_dir.y + a_workpiece.center.y, y_dir.z };
|
||||
a_workpiece.z_dir = { a_workpiece.center.x, a_workpiece.center.y, a_workpiece.center.z - 10 };
|
||||
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 };
|
||||
workpiecePositioning.push_back(a_workpiece);
|
||||
}
|
||||
|
||||
@ -438,6 +532,8 @@ void wd_workpieceHolePositioning(
|
||||
{
|
||||
rpt = _ptRotate(workpiecePositioning[i].holes[j], groundCalibPara.invRMatrix);
|
||||
workpiecePositioning[i].holes[j] = rpt;
|
||||
rpt = _ptRotate(workpiecePositioning[i].holesDir[j], groundCalibPara.invRMatrix);
|
||||
workpiecePositioning[i].holesDir[j] = rpt;
|
||||
}
|
||||
}
|
||||
return;
|
||||
|
||||
@ -17,6 +17,7 @@ typedef struct
|
||||
{
|
||||
int workpieceType;
|
||||
std::vector<SVzNL3DPoint> holes;
|
||||
std::vector<SVzNL3DPoint> holesDir;
|
||||
SVzNL3DPoint center;
|
||||
SVzNL3DPoint z_dir;
|
||||
SVzNL3DPoint y_dir;
|
||||
|
||||
@ -276,6 +276,7 @@ void _outputRGBDResult_RGBD(
|
||||
a_objPt.pt3D = workpiecePositioning[i].holes[j];
|
||||
objects.push_back(a_objPt);
|
||||
}
|
||||
a_objPt.pt3D = workpiecePositioning[i].center;
|
||||
objects.push_back(a_objPt);
|
||||
}
|
||||
|
||||
@ -342,11 +343,11 @@ void _outputRGBDResult_RGBD(
|
||||
int linePtNum = (int)objects.size();
|
||||
sw << "Line_" << lineNum << "_0_" << linePtNum + 1 << std::endl;
|
||||
lineNum++;
|
||||
size = 10;
|
||||
for (int i = 0; i < linePtNum; i++)
|
||||
{
|
||||
int colorIdx = objects[i].nPointIdx % 8;
|
||||
rgb = objColor[colorIdx];
|
||||
size = 10;
|
||||
float x = (float)objects[i].pt3D.x;
|
||||
float y = (float)objects[i].pt3D.y;
|
||||
float z = (float)objects[i].pt3D.z;
|
||||
@ -354,7 +355,57 @@ void _outputRGBDResult_RGBD(
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
|
||||
}
|
||||
//修正显示软件bug
|
||||
rgb = objColor[1];
|
||||
float x = (float)objects[0].pt3D.x;
|
||||
float y = (float)objects[0].pt3D.y;
|
||||
float z = (float)objects[0].pt3D.z;
|
||||
sw << "{" << x << "," << y << "," << z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
|
||||
//输出方向线条
|
||||
rgb = { 255, 0, 0 };
|
||||
size = 2;
|
||||
for (int i = 0; i < objNumber; i++)
|
||||
{
|
||||
sw << "Poly_" << lineIdx << "_2" << std::endl;
|
||||
sw << "{" << workpiecePositioning[i].center.x << "," << workpiecePositioning[i].center.y << "," << workpiecePositioning[i].center.z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
|
||||
sw << "{" << workpiecePositioning[i].y_dir.x << "," << workpiecePositioning[i].y_dir.y << "," << workpiecePositioning[i].y_dir.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 << "{" << workpiecePositioning[i].center.x << "," << workpiecePositioning[i].center.y << "," << workpiecePositioning[i].center.z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
|
||||
sw << "{" << workpiecePositioning[i].z_dir.x << "," << workpiecePositioning[i].z_dir.y << "," << workpiecePositioning[i].z_dir.z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
|
||||
lineIdx++;
|
||||
|
||||
for (int j = 0; j < (int)workpiecePositioning[i].holes.size(); j++)
|
||||
{
|
||||
sw << "Poly_" << lineIdx << "_2" << std::endl;
|
||||
sw << "{" << workpiecePositioning[i].holes[j].x << "," << workpiecePositioning[i].holes[j].y << "," << workpiecePositioning[i].holes[j].z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
|
||||
sw << "{" << workpiecePositioning[i].holesDir[j].x << "," << workpiecePositioning[i].holesDir[j].y << "," << workpiecePositioning[i].holesDir[j].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 << "{" << workpiecePositioning[0].center.x << "," << workpiecePositioning[0].center.y << "," << workpiecePositioning[0].center.z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
|
||||
sw << "{" << workpiecePositioning[0].y_dir.x << "," << workpiecePositioning[0].y_dir.y << "," << workpiecePositioning[0].y_dir.z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
|
||||
|
||||
|
||||
sw.close();
|
||||
}
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user