workpieceHolePositioning version 1.6.0 : 添加了玖瑞工件（单件）的姿态计算。
This commit is contained in:
parent
4001bb5c55
commit
2ed102c9db
@ -29,7 +29,8 @@
|
||||
//version 1.5.4 : 添加山东本事机电轮胎定位API。
|
||||
//version 1.5.5 : 山东本事机电轮胎定位。（1）添加了轮胎内径大小的输出， （2）按高度排序。最上面的目标排在第一个。
|
||||
//version 1.5.6 : 山东本事机电轮胎定位:优化了轮胎定位算法，使用圆扫描提取轮廓点。
|
||||
std::string m_strVersion = "HolePostion 1.5.6";
|
||||
//version 1.6.0 : 添加了玖瑞工件（单件）的姿态计算。
|
||||
std::string m_strVersion = "HolePostion 1.6.0";
|
||||
const char* wd_workpieceHolePositioningVersion(void)
|
||||
{
|
||||
return m_strVersion.c_str();
|
||||
@ -2824,7 +2825,8 @@ void _computeAnlgeScanData(
|
||||
angle = (angle / PI) * 180 + 180.0;
|
||||
double R = sqrt(pow(a_pt.y - centerPoint.y, 2) + pow(a_pt.x - centerPoint.x, 2));
|
||||
|
||||
int angleLine = (int)(angle / angleScale);
|
||||
int angleLine = (int)(angle / angleScale + 0.5);
|
||||
angleLine = angleLine % (int)polarScanData.size();
|
||||
|
||||
SWD_polarPt a_polarPt;
|
||||
a_polarPt.lineIdx = line;
|
||||
@ -4313,7 +4315,7 @@ void Jiurui_getWorkpiecePose(
|
||||
centroid.z = centroid.z / centroid_size;
|
||||
|
||||
//圆周扫描:0.5度间隔
|
||||
double angleScale = 0.5;
|
||||
double angleScale = 1.0;
|
||||
std::vector<std::vector<SWD_polarPt>> polarScanData;
|
||||
int polarLines = (int)(360.0 / angleScale + 0.5);
|
||||
polarScanData.resize(polarLines);
|
||||
@ -4330,12 +4332,17 @@ void Jiurui_getWorkpiecePose(
|
||||
}
|
||||
|
||||
//提取平面点-计算法向
|
||||
double distToPlane_min = 17.5;
|
||||
double distToPlane_max = 18.5;
|
||||
double distToPlane_min = 19.5;
|
||||
double distToPlane_max = 20.5;
|
||||
std::vector<SWD_polarPt> planePolarPoints;
|
||||
std::vector<cv::Point3d> Points3ds;
|
||||
for (int i = 0; i < polarLines; i++)
|
||||
{
|
||||
double angle = angleScale * i;
|
||||
if ((angle < 30) || ((angle > 75) && (angle < 120)) || ((angle > 150) && (angle < 210)) ||
|
||||
((angle > 240) && (angle < 300)) || (angle > 330)) //避开水平和垂直区域
|
||||
continue;
|
||||
|
||||
int lineSize = (int)polarScanData[i].size();
|
||||
if (lineSize == 0)
|
||||
continue;
|
||||
@ -4367,6 +4374,8 @@ void Jiurui_getWorkpiecePose(
|
||||
|
||||
//计算投影向量
|
||||
SVzNL3DPoint vec_1 = { res[0], res[1], res[2]};
|
||||
if (res[2] < 0)
|
||||
vec_1 = { -res[0], -res[1], -res[2] };
|
||||
SVzNL3DPoint vec_z = { 0, 0, 1.0 };
|
||||
SSG_planeCalibPara poseR = wd_computeRTMatrix(vec_1, vec_z);
|
||||
|
||||
@ -4385,19 +4394,22 @@ void Jiurui_getWorkpiecePose(
|
||||
//
|
||||
//迭代计算质心
|
||||
centroid = { 0, 0, 0 };
|
||||
for (int i = 0; i < centroid_size; i++)
|
||||
std::vector< SVzNL2DPointD> points_2d;
|
||||
points_2d.resize(projectPosition3ds.size());
|
||||
for (int i = 0; i < (int)projectPosition3ds.size(); i++)
|
||||
{
|
||||
int line = objClusters[topClusterId][i].nPointIdx >> 16;
|
||||
int ptIdx = objClusters[topClusterId][i].nPointIdx & 0x0000FFFF;
|
||||
centroid.x += scanLinesInput[line][ptIdx].pt3D.x;
|
||||
centroid.y += scanLinesInput[line][ptIdx].pt3D.y;
|
||||
centroid.z += scanLinesInput[line][ptIdx].pt3D.z;
|
||||
centroid.x += projectPosition3ds[i].pt3D.x;
|
||||
centroid.y += projectPosition3ds[i].pt3D.y;
|
||||
centroid.z += projectPosition3ds[i].pt3D.z;
|
||||
points_2d[i] = { projectPosition3ds[i].pt3D.x , projectPosition3ds[i].pt3D.y };
|
||||
}
|
||||
centroid.x = centroid.x / centroid_size;
|
||||
centroid.y = centroid.y / centroid_size;
|
||||
centroid.z = centroid.z / centroid_size;
|
||||
|
||||
//圆周扫描:0.5度间隔
|
||||
angleScale = 1.0;
|
||||
polarLines = (int)(360.0 / angleScale + 0.5);
|
||||
polarScanData.clear();
|
||||
polarScanData.resize(polarLines);
|
||||
_computeAnlgeScanData(
|
||||
@ -4429,30 +4441,43 @@ void Jiurui_getWorkpiecePose(
|
||||
objCenter.y = objCenter.y / centerSum;
|
||||
objCenter.z = objCenter.z / centerSum;
|
||||
|
||||
//计算长轴方向
|
||||
#if 0
|
||||
//计算长轴方向: 长轴短轴比最大
|
||||
int degree90 = (int)(90.0 / angleScale);
|
||||
int degree180 = (int)(180.0 / angleScale);
|
||||
int longAxisIndex = -1;
|
||||
double longAxisLen = -1;
|
||||
double axisLenRatio = 0;
|
||||
std::vector<double> ratioBuff;
|
||||
ratioBuff.resize(polarLines / 2);
|
||||
std::vector<double> sumRBuff;
|
||||
sumRBuff.resize(polarLines / 2);
|
||||
for (int i = 0; i < polarLines/2; i++)
|
||||
{
|
||||
int idx_1 = i;
|
||||
int idx_2 = i + degree180;
|
||||
int idx_2 = (i + degree180) % polarLines;
|
||||
int idx_3 = (i + degree90) % polarLines;
|
||||
int idx_4 = (idx_3 + degree180) % polarLines;
|
||||
if (idx_2 < polarLines)
|
||||
{
|
||||
if ((polarScanData[idx_1].size() == 0) || (polarScanData[idx_2].size() == 0))
|
||||
if ((polarScanData[idx_1].size() > 0) && (polarScanData[idx_2].size() > 0) &&
|
||||
(polarScanData[idx_3].size() > 0) && (polarScanData[idx_4].size() > 0))
|
||||
{
|
||||
double sumR = polarScanData[idx_1][0].R + polarScanData[idx_2][0].R;
|
||||
double sumR_long = polarScanData[idx_1].back().R + polarScanData[idx_2].back().R;
|
||||
double sumR_short = polarScanData[idx_3].back().R + polarScanData[idx_4].back().R;
|
||||
double ratio = sumR_long / sumR_short;
|
||||
sumRBuff[i] = sumR_long;
|
||||
ratioBuff[i] = ratio;
|
||||
if (longAxisIndex < 0)
|
||||
{
|
||||
longAxisIndex = idx_1;
|
||||
longAxisLen = sumR;
|
||||
axisLenRatio = ratio;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (longAxisLen < sumR)
|
||||
if (axisLenRatio < ratio)
|
||||
{
|
||||
longAxisIndex = idx_1;
|
||||
longAxisLen = sumR;
|
||||
axisLenRatio = ratio;
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -4464,8 +4489,14 @@ void Jiurui_getWorkpiecePose(
|
||||
return;
|
||||
}
|
||||
|
||||
double longAxisAngle = longAxisIndex * angleScale;
|
||||
SVzNL3DPoint vector_y = {cos(longAxisAngle/PI), sin(longAxisAngle / PI), 0 };
|
||||
double longAxisAngle = longAxisIndex * angleScale * PI / 180;
|
||||
SVzNL3DPoint vector_y = { cos(longAxisAngle), sin(longAxisAngle), 0 };
|
||||
#else
|
||||
//PCA方法
|
||||
SVzNL2DPointD axis, roi_centroid;
|
||||
pca2D(points_2d, axis, roi_centroid);
|
||||
SVzNL3DPoint vector_y = { axis.x, axis.y, 0 };
|
||||
#endif
|
||||
//叉乘出vector_x
|
||||
SVzNL3DPoint vector_x;
|
||||
vector_x.x = vector_y.y * vec_z.z - vec_z.y * vector_y.z;
|
||||
|
||||
@ -1701,11 +1701,34 @@ void JiuRui_workpiecePosition_test(void)
|
||||
{
|
||||
//fidx =17;
|
||||
char _scan_file[256];
|
||||
sprintf_s(_scan_file, "%s%d_LaserData_Hi229229.txt", dataPath[grp], fidx);
|
||||
sprintf_s(_scan_file, "%s%d_LaserData_Hi229229_sampleDown.txt", dataPath[grp], fidx);
|
||||
std::vector<std::vector< SVzNL3DPosition>> scanLines;
|
||||
vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines);
|
||||
if (scanLines.size() == 0)
|
||||
continue;
|
||||
#if 0
|
||||
//抽样
|
||||
std::vector<std::vector< SVzNL3DPosition>> sampleData;
|
||||
int sampleLines = (int)scanLines.size() / 4;
|
||||
sampleData.resize(sampleLines);
|
||||
int sLine = 0;
|
||||
for (int i = 0; i < (int)scanLines.size(); i++)
|
||||
{
|
||||
if (i % 4 == 0)
|
||||
{
|
||||
sampleData[sLine].resize(scanLines[i].size());
|
||||
std::copy(scanLines[i].begin(), scanLines[i].end(), sampleData[sLine].begin()); // 使用std::copy算法
|
||||
sLine++;
|
||||
if (sLine >= sampleLines)
|
||||
break;
|
||||
}
|
||||
}
|
||||
//写入
|
||||
sprintf_s(_scan_file, "%s%d_LaserData_Hi229229_sampleDown.txt", dataPath[grp], fidx);
|
||||
int headNullLines = 0;
|
||||
_outputScanDataFile_vector(_scan_file, sampleData, false, &headNullLines);
|
||||
continue;
|
||||
#endif
|
||||
|
||||
long t1 = (long)GetTickCount64();//统计时间
|
||||
|
||||
@ -1766,9 +1789,9 @@ int main()
|
||||
//ESG_testMode testMode = keSG_华航孔定位;
|
||||
//ESG_testMode testMode = keSG_微力砂轮盘定位;
|
||||
//ESG_testMode testMode = keSG_微力砂轮架子定位;
|
||||
ESG_testMode testMode = keSG_山东本事轮胎定位;
|
||||
//ESG_testMode testMode = keSG_山东本事轮胎定位;
|
||||
//ESG_testMode testMode = keSG_宁波海瑞马转子芯定位;
|
||||
//ESG_testMode testMode = keSG_北京玖瑞工件定位;
|
||||
ESG_testMode testMode = keSG_北京玖瑞工件定位;
|
||||
|
||||
if (keSG_拓普发孔定位 == testMode)
|
||||
TuoPuFa_holePosition_test();
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user