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

workpieceHolePositioning v1.2.0

Browse Source 
算法完成了6轴验证
This commit is contained in:
jerryzeng 2026-02-02 15:10:42 +08:00
parent 4f9d6360af
commit 11b93b92df
2 changed files with 81 additions and 69 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

125
sourceCode/workpieceHolePositioning.cpp
View File

@ -1,4 +1,4 @@
#include <vector> #include <vector>
#include "SG_baseDataType.h" #include "SG_baseDataType.h"
#include "SG_baseAlgo_Export.h" #include "SG_baseAlgo_Export.h"
#include "workpieceHolePositioning_Export.h" #include "workpieceHolePositioning_Export.h"
@ -6,24 +6,25 @@
#include <limits> #include <limits>
//version 1.0.0 : base version release to customer //version 1.0.0 : base version release to customer
//version 1.0.2 : 添加了工件姿态（欧拉角输出) //version 1.0.2 : 添加了工件姿态（欧拉角输出)
//version 1.1.0 : c对工件姿态规范化为中心点（操作点）加三个方向矢量 //version 1.1.0 : c对工件姿态规范化为中心点（操作点）加三个方向矢量
std::string m_strVersion = "1.1.1"; //version 1.2.0 : 算法完成了6轴验证
std::string m_strVersion = "1.2.0";
const char* wd_workpieceHolePositioningVersion(void) const char* wd_workpieceHolePositioningVersion(void)
{ {
return m_strVersion.c_str(); return m_strVersion.c_str();
} }
//相机水平安装计算地面调平参数。 //相机水平安装计算地面调平参数。
//相机Z轴基本平行地面时，需要以地面为参照，将相机调水平 //相机Z轴基本平行地面时，需要以地面为参照，将相机调水平
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数 //旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
SSG_planeCalibPara wd_getGroundCalibPara( SSG_planeCalibPara wd_getGroundCalibPara(
std::vector< std::vector<SVzNL3DPosition>>& scanLines) std::vector< std::vector<SVzNL3DPosition>>& scanLines)
{ {
return sg_getPlaneCalibPara2(scanLines); return sg_getPlaneCalibPara2(scanLines);
} }
//相机水平时姿态调平，并去除地面 //相机水平时姿态调平，并去除地面
void wd_lineDataR( void wd_lineDataR(
std::vector< SVzNL3DPosition>& a_line, std::vector< SVzNL3DPosition>& a_line,
const double* camPoseR, const double* camPoseR,
@ -41,7 +42,7 @@ SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, const double matrix3d[9])
return _r_pt; return _r_pt;
} }
//搜索最接近distance的目标 //搜索最接近distance的目标
int distanceSearchObject(SVzNL3DPoint seed, std::vector<SWD_HoleInfo>& holes, double distance, double distDeviation) int distanceSearchObject(SVzNL3DPoint seed, std::vector<SWD_HoleInfo>& holes, double distance, double distDeviation)
{ {
int result = -1; int result = -1;
@ -66,7 +67,7 @@ int distanceSearchObject(SVzNL3DPoint seed, std::vector<SWD_HoleInfo>& holes, do
return result; return result;
} }
//搜索最接近distance且角度为angle的目标, 以角度为优先 //搜索最接近distance且角度为angle的目标, 以角度为优先
int angleConditionDistanceSearch( int angleConditionDistanceSearch(
SVzNL3DPoint seed, SVzNL3DPoint angleSide, SVzNL3DPoint seed, SVzNL3DPoint angleSide,
std::vector<SWD_HoleInfo>& holes, std::vector<SWD_HoleInfo>& holes,
@ -151,7 +152,7 @@ double _getMeanZ(std::vector<std::vector<double>>& quantiValue, SVzNL3DPoint see
return (zSum / hist); return (zSum / hist);
} }
//工件孔定位 //工件孔定位
void wd_workpieceHolePositioning( void wd_workpieceHolePositioning(
std::vector< std::vector<SVzNL3DPosition>>& scanLinesInput, std::vector< std::vector<SVzNL3DPosition>>& scanLinesInput,
const WD_workpieceHoleParam workpiecePara, const WD_workpieceHoleParam workpiecePara,
@ -175,20 +176,20 @@ void wd_workpieceHolePositioning(
isGridData = false; isGridData = false;
scanLines[i].resize(scanLinesInput[i].size()); scanLines[i].resize(scanLinesInput[i].size());
std::copy(scanLinesInput[i].begin(), scanLinesInput[i].end(), scanLines[i].begin()); // 使用std::copy算法 std::copy(scanLinesInput[i].begin(), scanLinesInput[i].end(), scanLines[i].begin()); // 使用std::copy算法
} }
if (false == isGridData)//数据不是网格格式 if (false == isGridData)//数据不是网格格式
{ {
*errCode = SG_ERR_NOT_GRID_FORMAT; *errCode = SG_ERR_NOT_GRID_FORMAT;
return; return;
} }
for (int i = 0; i < lineNum; i++) for (int i = 0; i < lineNum; i++)
{ //行处理 { //行处理
//调平，去除地面 //调平，去除地面
wd_lineDataR(scanLines[i], groundCalibPara.planeCalib, -1); wd_lineDataR(scanLines[i], groundCalibPara.planeCalib, -1);
} }
//生成量化数据，以1mm为量化尺度，用于确定工件表面高度 //生成量化数据，以1mm为量化尺度，用于确定工件表面高度
SVzNL3DRangeD roi3D = sg_getScanDataROI_vector( scanLines); SVzNL3DRangeD roi3D = sg_getScanDataROI_vector( scanLines);
SVzNLRect roi2D; SVzNLRect roi2D;
roi2D.left = (int)roi3D.xRange.min; roi2D.left = (int)roi3D.xRange.min;
@ -204,11 +205,11 @@ void wd_workpieceHolePositioning(
for (int i = 0; i < quanti_X; i++) for (int i = 0; i < quanti_X; i++)
{ {
quantiValue[i].resize(quanti_Y); quantiValue[i].resize(quanti_Y);
std::fill(quantiValue[i].begin(), quantiValue[i].end(), 0);//初始化为0 std::fill(quantiValue[i].begin(), quantiValue[i].end(), 0);//初始化为0
quantiHist[i].resize(quanti_Y); quantiHist[i].resize(quanti_Y);
std::fill(quantiHist[i].begin(), quantiHist[i].end(), 0);//初始化为0 std::fill(quantiHist[i].begin(), quantiHist[i].end(), 0);//初始化为0
} }
//以1mm尺度量化 //以1mm尺度量化
for (int line = 0; line < lineNum; line++) for (int line = 0; line < lineNum; line++)
{ {
for (int j = 0; j < linePtNum; j++) for (int j = 0; j < linePtNum; j++)
@ -236,7 +237,7 @@ void wd_workpieceHolePositioning(
pointMask.resize(lineNum); pointMask.resize(lineNum);
std::vector<SVzNL3DPoint> endingPoints; std::vector<SVzNL3DPoint> endingPoints;
//提取线段端点特征 //提取线段端点特征
for (int line = 0; line < lineNum; line++) for (int line = 0; line < lineNum; line++)
{ {
if (line == 1677) if (line == 1677)
@ -244,8 +245,8 @@ void wd_workpieceHolePositioning(
std::vector<SVzNL3DPosition>& lineData = scanLines[line]; std::vector<SVzNL3DPosition>& lineData = scanLines[line];
pointMask[line].resize(lineData.size()); pointMask[line].resize(lineData.size());
std::fill(pointMask[line].begin(), pointMask[line].end(), 0);//初始化为0 std::fill(pointMask[line].begin(), pointMask[line].end(), 0);//初始化为0
//滤波，滤除异常点 //滤波，滤除异常点
sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam); sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam);
std::vector<SSG_RUN> segs; std::vector<SSG_RUN> segs;
@ -253,19 +254,19 @@ void wd_workpieceHolePositioning(
lineData, lineData,
lineSegPara, lineSegPara,
segs); segs);
//将seg端点作为边缘点。做了地面调平后，垂直孔的内侧在XY平面上均为边缘点。 //将seg端点作为边缘点。做了地面调平后，垂直孔的内侧在XY平面上均为边缘点。
for (int i = 0, i_max = (int)segs.size(); i < i_max; i++) for (int i = 0, i_max = (int)segs.size(); i < i_max; i++)
{ {
int ptIdx = segs[i].start; int ptIdx = segs[i].start;
endingPoints.push_back(lineData[ptIdx].pt3D); endingPoints.push_back(lineData[ptIdx].pt3D);
pointMask[line][ptIdx] = 1; //防止重复 pointMask[line][ptIdx] = 1; //防止重复
ptIdx = segs[i].start + segs[i].len - 1; ptIdx = segs[i].start + segs[i].len - 1;
endingPoints.push_back(lineData[ptIdx].pt3D); endingPoints.push_back(lineData[ptIdx].pt3D);
pointMask[line][ptIdx] = 1; pointMask[line][ptIdx] = 1;
} }
} }
//生成水平扫描 //生成水平扫描
std::vector<std::vector<SVzNL3DPosition>> hLines_raw; std::vector<std::vector<SVzNL3DPosition>> hLines_raw;
hLines_raw.resize(linePtNum); hLines_raw.resize(linePtNum);
for (int i = 0; i < linePtNum; i++) for (int i = 0; i < linePtNum; i++)
@ -274,20 +275,20 @@ void wd_workpieceHolePositioning(
{ {
for (int j = 0; j < linePtNum; j++) for (int j = 0; j < linePtNum; j++)
{ {
scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用） scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
hLines_raw[j][line] = scanLines[line][j]; hLines_raw[j][line] = scanLines[line][j];
hLines_raw[j][line].pt3D.x = scanLines[line][j].pt3D.y; hLines_raw[j][line].pt3D.x = scanLines[line][j].pt3D.y;
hLines_raw[j][line].pt3D.y = scanLines[line][j].pt3D.x; hLines_raw[j][line].pt3D.y = scanLines[line][j].pt3D.x;
} }
} }
//水平arc特征提取 //水平arc特征提取
int lineNum_h_raw = (int)hLines_raw.size(); int lineNum_h_raw = (int)hLines_raw.size();
for (int line = 0; line < lineNum_h_raw; line++) for (int line = 0; line < lineNum_h_raw; line++)
{ {
if (line == 974) if (line == 974)
int kkk = 1; int kkk = 1;
std::vector<SVzNL3DPosition>& lineData = hLines_raw[line]; std::vector<SVzNL3DPosition>& lineData = hLines_raw[line];
//滤波，滤除异常点 //滤波，滤除异常点
int ptNum = (int)lineData.size(); int ptNum = (int)lineData.size();
sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], ptNum, filterParam); sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], ptNum, filterParam);
@ -296,11 +297,11 @@ void wd_workpieceHolePositioning(
lineData, lineData,
lineSegPara, lineSegPara,
segs); segs);
//将seg端点作为边缘点。做了地面调平后，垂直孔的内侧在XY平面上均为边缘点。 //将seg端点作为边缘点。做了地面调平后，垂直孔的内侧在XY平面上均为边缘点。
for (int i = 0, i_max = (int)segs.size(); i < i_max; i++) for (int i = 0, i_max = (int)segs.size(); i < i_max; i++)
{ {
int ptIdx = segs[i].start; int ptIdx = segs[i].start;
if (pointMask[ptIdx][line] == 0) //防止点重复 if (pointMask[ptIdx][line] == 0) //防止点重复
{ {
SVzNL3DPoint an_ending; SVzNL3DPoint an_ending;
an_ending.x = lineData[ptIdx].pt3D.y; an_ending.x = lineData[ptIdx].pt3D.y;
@ -310,7 +311,7 @@ void wd_workpieceHolePositioning(
pointMask[ptIdx][line] = 1; pointMask[ptIdx][line] = 1;
} }
ptIdx = segs[i].start + segs[i].len - 1; ptIdx = segs[i].start + segs[i].len - 1;
if (pointMask[ptIdx][line] == 0) //防止点重复 if (pointMask[ptIdx][line] == 0) //防止点重复
{ {
SVzNL3DPoint an_ending; SVzNL3DPoint an_ending;
an_ending.x = lineData[ptIdx].pt3D.y; an_ending.x = lineData[ptIdx].pt3D.y;
@ -322,7 +323,7 @@ void wd_workpieceHolePositioning(
} }
} }
//标注 //标注
std::vector<std::vector<SSG_featureClusteringInfo>> featureInfoMask; std::vector<std::vector<SSG_featureClusteringInfo>> featureInfoMask;
std::vector<std::vector<SVzNL3DPoint>> feature3DInfo; std::vector<std::vector<SVzNL3DPoint>> feature3DInfo;
featureInfoMask.resize(lineNum); featureInfoMask.resize(lineNum);
@ -333,7 +334,7 @@ void wd_workpieceHolePositioning(
feature3DInfo[i].resize(lineNum_h_raw); feature3DInfo[i].resize(lineNum_h_raw);
} }
//标注 //标注
for (int line = 0; line < lineNum; line++) for (int line = 0; line < lineNum; line++)
{ {
std::vector<int>& a_lineMask = pointMask[line]; std::vector<int>& a_lineMask = pointMask[line];
@ -353,9 +354,9 @@ void wd_workpieceHolePositioning(
} }
} }
} }
//聚类 //聚类
//采用迭代思想，回归思路进行高效聚类 //采用迭代思想，回归思路进行高效聚类
std::vector<std::vector< SVzNL2DPoint>> clusters; //只记录位置 std::vector<std::vector< SVzNL2DPoint>> clusters; //只记录位置
std::vector<SVzNL3DRangeD> clustersRoi3D; std::vector<SVzNL3DRangeD> clustersRoi3D;
int clusterID = 1; int clusterID = 1;
int clusterCheckWin = 5; int clusterCheckWin = 5;
@ -364,7 +365,7 @@ void wd_workpieceHolePositioning(
for (int x = 0; x < lineNum; x++) for (int x = 0; x < lineNum; x++)
{ {
SSG_featureClusteringInfo& a_featureInfo = featureInfoMask[x][y]; SSG_featureClusteringInfo& a_featureInfo = featureInfoMask[x][y];
if ((0 == a_featureInfo.featurType) || (a_featureInfo.clusterID > 0)) //非特征或已经处理 if ((0 == a_featureInfo.featurType) || (a_featureInfo.clusterID > 0)) //非特征或已经处理
continue; continue;
SVzNL3DPoint& a_feature3DValue = feature3DInfo[x][y]; SVzNL3DPoint& a_feature3DValue = feature3DInfo[x][y];
@ -380,11 +381,11 @@ void wd_workpieceHolePositioning(
std::vector< SVzNL2DPoint> a_cluster; std::vector< SVzNL2DPoint> a_cluster;
a_cluster.push_back(a_seedPos); a_cluster.push_back(a_seedPos);
wd_gridPointClustering( wd_gridPointClustering(
featureInfoMask,//int，记录特征标记和clusterID，附加一个flag featureInfoMask,//int，记录特征标记和clusterID，附加一个flag
feature3DInfo,//double,记录坐标信息 feature3DInfo,//double,记录坐标信息
clusterCheckWin, //搜索窗口 clusterCheckWin, //搜索窗口
growParam,//聚类条件 growParam,//聚类条件
clusterID, //当前Cluster的ID clusterID, //当前Cluster的ID
a_cluster, //result a_cluster, //result
a_clusterRoi a_clusterRoi
); );
@ -393,7 +394,7 @@ void wd_workpieceHolePositioning(
clusterID++; clusterID++;
} }
} }
//聚类结果分析 //聚类结果分析
std::vector<int> validCluserIndexing; std::vector<int> validCluserIndexing;
int clusterSize = (int)clusters.size(); int clusterSize = (int)clusters.size();
for (int i = 0; i < clusterSize; i++) for (int i = 0; i < clusterSize; i++)
@ -405,14 +406,14 @@ void wd_workpieceHolePositioning(
(W > workpiecePara.holeDiameter * 0.5) && (W < workpiecePara.holeDiameter * 2)) (W > workpiecePara.holeDiameter * 0.5) && (W < workpiecePara.holeDiameter * 2))
validCluserIndexing.push_back(i); validCluserIndexing.push_back(i);
} }
//生成结果 //生成结果
std::vector< SWD_HoleInfo> holes; std::vector< SWD_HoleInfo> holes;
int objectSize = (int)validCluserIndexing.size(); int objectSize = (int)validCluserIndexing.size();
for (int objIdx = 0; objIdx < objectSize; objIdx++) for (int objIdx = 0; objIdx < objectSize; objIdx++)
{ {
std::vector<SVzNL3DPoint> pointArray; std::vector<SVzNL3DPoint> pointArray;
int clusterIdx = validCluserIndexing[objIdx]; int clusterIdx = validCluserIndexing[objIdx];
//取cluster上的点 //取cluster上的点
int clusterPtSize = (int)clusters[clusterIdx].size(); int clusterPtSize = (int)clusters[clusterIdx].size();
double minZ = DBL_MAX; double minZ = DBL_MAX;
for (int i = 0; i < clusterPtSize; i++) for (int i = 0; i < clusterPtSize; i++)
@ -426,7 +427,7 @@ void wd_workpieceHolePositioning(
minZ = a_pt3d.z; minZ = a_pt3d.z;
pointArray.push_back(a_pt3d); pointArray.push_back(a_pt3d);
} }
//圆拟合 //圆拟合
SVzNL3DPoint center; SVzNL3DPoint center;
double radius; double radius;
double err = fitCircleByLeastSquare(pointArray, center, radius); double err = fitCircleByLeastSquare(pointArray, center, radius);
@ -436,9 +437,9 @@ void wd_workpieceHolePositioning(
a_hole.radius = radius; a_hole.radius = radius;
holes.push_back(a_hole); holes.push_back(a_hole);
} }
//分割 //分割
//方法：先搜索与W最接近的点，然后条件搜索（垂直）与L最接近的点 //方法：先搜索与W最接近的点，然后条件搜索（垂直）与L最接近的点
double distDeviation = 5.0; //距离搜索的合格门限。小于此距离，认为搜索到的目标为有效 double distDeviation = 5.0; //距离搜索的合格门限。小于此距离，认为搜索到的目标为有效
for (int objIdx = 0; objIdx < objectSize; objIdx++) for (int objIdx = 0; objIdx < objectSize; objIdx++)
{ {
if (holes[objIdx].radius < 0) if (holes[objIdx].radius < 0)
@ -450,9 +451,9 @@ void wd_workpieceHolePositioning(
if (idx1 < 0) if (idx1 < 0)
continue; continue;
SVzNLRangeD angleRange = { 85, 95 }; //垂直，5度范围 SVzNLRangeD angleRange = { 85, 95 }; //垂直，5度范围
SWD_HoleInfo& p1 = holes[idx1]; SWD_HoleInfo& p1 = holes[idx1];
//搜索最接近distance且角度为angle的目标, 以角度为优先 //搜索最接近distance且角度为angle的目标, 以角度为优先
int idx2 = angleConditionDistanceSearch( int idx2 = angleConditionDistanceSearch(
p0.center, p1.center, p0.center, p1.center,
holes, holes,
@ -462,7 +463,7 @@ void wd_workpieceHolePositioning(
continue; continue;
SWD_HoleInfo& p2 = holes[idx2]; SWD_HoleInfo& p2 = holes[idx2];
//搜索最接近distance且角度为angle的目标, 以角度为优先 //搜索最接近distance且角度为angle的目标, 以角度为优先
int idx3 = angleConditionDistanceSearch( int idx3 = angleConditionDistanceSearch(
p1.center, p0.center, p1.center, p0.center,
holes, holes,
@ -475,7 +476,7 @@ void wd_workpieceHolePositioning(
p2.radius = -1; p2.radius = -1;
p3.radius = -1; p3.radius = -1;
//重新计算Z值。因为沉孔的原因，Z值会不准确。取四条边的中点处的Z值的均值作为整个的Z值 //重新计算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_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_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_p1p3 = { (p1.center.x + p3.center.x) / 2,(p1.center.y + p3.center.y) / 2, (p1.center.z + p3.center.z) / 2 };
@ -500,7 +501,7 @@ void wd_workpieceHolePositioning(
for (int m = 0; m < 4; m++) for (int m = 0; m < 4; m++)
{ {
SVzNL3DPoint a_pt = a_workpiece.holes[m]; SVzNL3DPoint a_pt = a_workpiece.holes[m];
a_pt.z = a_pt.z + 20; //法向，因为做过地面高平，所以法向只在z向 a_pt.z = a_pt.z + 20; //法向，因为做过地面高平，所以法向只在z向
a_workpiece.holesDir.push_back(a_pt); a_workpiece.holesDir.push_back(a_pt);
} }
a_workpiece.center = { (p0.center.x + p1.center.x + p2.center.x + p3.center.x) / 4, a_workpiece.center = { (p0.center.x + p1.center.x + p2.center.x + p3.center.x) / 4,
@ -508,10 +509,10 @@ void wd_workpieceHolePositioning(
(z1 + z2 + z3 + z4) / 4 }; (z1 + z2 + z3 + z4) / 4 };
SVzNL3DPoint y_dir; SVzNL3DPoint y_dir;
if (p0.center.y < p2.center.y) if (p0.center.x < p1.center.x)
y_dir = { p2.center.x - p0.center.x, p2.center.y - p0.center.y, 0 }; y_dir = { p1.center.x - p0.center.x, p1.center.y - p0.center.y, 0 };
else else
y_dir = { p0.center.x - p2.center.x, p0.center.y - p2.center.y, 0 }; y_dir = { p0.center.x - p1.center.x, p0.center.y - p1.center.y, 0 };
double modLen = sqrt(pow(y_dir.x, 2) + pow(y_dir.y, 2)); double modLen = sqrt(pow(y_dir.x, 2) + pow(y_dir.y, 2));
y_dir = { y_dir.x / modLen, y_dir.y / modLen, 0 }; 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.y_dir = { y_dir.x * 20 + a_workpiece.center.x, y_dir.y * 20 + a_workpiece.center.y, a_workpiece.center.z };
@ -521,7 +522,7 @@ void wd_workpieceHolePositioning(
} }
int workpieceNum = (int)workpiecePositioning.size(); int workpieceNum = (int)workpiecePositioning.size();
//旋转回去 //旋转回去
for (int i = 0; i < workpieceNum; i++) for (int i = 0; i < workpieceNum; i++)
{ {
SVzNL3DPoint rpt; SVzNL3DPoint rpt;
@ -546,10 +547,10 @@ void wd_workpieceHolePositioning(
workpiecePositioning[i].y_dir.y - workpiecePositioning[i].center.y, workpiecePositioning[i].y_dir.y - workpiecePositioning[i].center.y,
workpiecePositioning[i].y_dir.z - workpiecePositioning[i].center.z }; 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)); 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 }; //归一化 vector_z = { vector_z.x / mod_vz, vector_z.y / mod_vz, vector_z.z / mod_vz }; //归一化
double mod_vy = sqrt(pow(vector_y.x, 2) + pow(vector_y.y, 2) + pow(vector_y.z, 2)); 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 }; //归一化 vector_y = { vector_y.x / mod_vy, vector_y.y / mod_vy, vector_y.z / mod_vy }; //归一化
//叉乘出vector_x //叉乘出vector_x
SVzNL3DPoint vector_x; SVzNL3DPoint vector_x;
vector_x.x = vector_y.y * vector_z.z - vector_z.y * vector_y.z; 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.y = vector_y.z * vector_z.x - vector_z.z * vector_y.x;
@ -558,7 +559,7 @@ void wd_workpieceHolePositioning(
workpiecePositioning[i].y_dir = vector_y; workpiecePositioning[i].y_dir = vector_y;
workpiecePositioning[i].z_dir = vector_z; workpiecePositioning[i].z_dir = vector_z;
#if 0 #if 0
//得到旋转矩阵 //得到旋转矩阵
double R[3][3]; double R[3][3];
R[0][0] = vector_x.x; R[0][0] = vector_x.x;
R[1][0] = vector_x.y; R[1][0] = vector_x.y;

25
workpieceHolePositioning_test/workpieceHolePositioning_test.cpp
View File

@ -154,6 +154,12 @@ void _outputWorkpieceInfo(char* fileName, std::vector< WD_workpieceInfo>& workpi
} }
sprintf_s(dataStr, 50, " center: (%g, %g, %g)", workpiecePositioning[i].center.x, workpiecePositioning[i].center.y, workpiecePositioning[i].center.z); sprintf_s(dataStr, 50, " center: (%g, %g, %g)", workpiecePositioning[i].center.x, workpiecePositioning[i].center.y, workpiecePositioning[i].center.z);
sw << dataStr << std::endl; sw << dataStr << std::endl;
sprintf_s(dataStr, 50, " x_dir: (%g, %g, %g)", workpiecePositioning[i].x_dir.x, workpiecePositioning[i].x_dir.y, workpiecePositioning[i].x_dir.z);
sw << dataStr << std::endl;
sprintf_s(dataStr, 50, " y_dir: (%g, %g, %g)", workpiecePositioning[i].y_dir.x, workpiecePositioning[i].y_dir.y, workpiecePositioning[i].y_dir.z);
sw << dataStr << std::endl;
sprintf_s(dataStr, 50, " z_dir: (%g, %g, %g)", workpiecePositioning[i].z_dir.x, workpiecePositioning[i].z_dir.y, workpiecePositioning[i].z_dir.z);
sw << dataStr << std::endl;
} }
sw.close(); sw.close();
} }
@ -444,7 +450,7 @@ int main()
}; };
SVzNLRange fileIdx[TEST_GROUP] = { SVzNLRange fileIdx[TEST_GROUP] = {
{5,5}, {6,6},
}; };
const char* ver = wd_workpieceHolePositioningVersion(); const char* ver = wd_workpieceHolePositioningVersion();
@ -494,9 +500,13 @@ int main()
std::cout << T << std::endl; std::cout << T << std::endl;
//验算6轴姿态 //验算6轴姿态
std::vector<cv::Point3d> verify_pts_eye;
verify_pts_eye.insert(verify_pts_eye.end(), pts_eye.begin(), pts_eye.end());
cv::Point3d a_center = { 232.997, -173.533, 1795.9 };
verify_pts_eye.push_back(a_center);
std::vector<std::vector< cv::Point3d>> pose_eye; std::vector<std::vector< cv::Point3d>> pose_eye;
pose_eye.resize(6); pose_eye.resize(7);
for (int i = 0; i < 6; i++) for (int i = 0; i < 7; i++)
pose_eye[i].resize(3); pose_eye[i].resize(3);
pose_eye[0][0] = { -0.020, -1.000, -0.011 }; pose_eye[0][1] = { 1.000, -0.020, -0.001 }; pose_eye[0][2] = { 0.001, -0.011, 1.000 }; pose_eye[0][0] = { -0.020, -1.000, -0.011 }; pose_eye[0][1] = { 1.000, -0.020, -0.001 }; pose_eye[0][2] = { 0.001, -0.011, 1.000 };
pose_eye[1][0] = { 0.021,-1.000,-0.011 }; pose_eye[1][1] = { 1.000,0.021,-0.000 }; pose_eye[1][2] = { 0.001,-0.011,1.000 }; pose_eye[1][0] = { 0.021,-1.000,-0.011 }; pose_eye[1][1] = { 1.000,0.021,-0.000 }; pose_eye[1][2] = { 0.001,-0.011,1.000 };
@ -504,13 +514,14 @@ int main()
pose_eye[3][0] = { 0.008,-1.000,-0.011 }; pose_eye[3][1] = { 1.000,0.008,-0.000 }; pose_eye[3][2] = { 0.001,-0.011,1.000 }; pose_eye[3][0] = { 0.008,-1.000,-0.011 }; pose_eye[3][1] = { 1.000,0.008,-0.000 }; pose_eye[3][2] = { 0.001,-0.011,1.000 };
pose_eye[4][0] = { 0.006,-1.000,-0.011 }; pose_eye[4][1] = { 1.000,0.006,-0.000 }; pose_eye[4][2] = { 0.001,-0.011,1.000 }; pose_eye[4][0] = { 0.006,-1.000,-0.011 }; pose_eye[4][1] = { 1.000,0.006,-0.000 }; pose_eye[4][2] = { 0.001,-0.011,1.000 };
pose_eye[5][0] = { 0.139,-0.990,-0.011 }; pose_eye[5][1] = { 0.990,0.139,0.001 }; pose_eye[5][2] = { 0.001,-0.011,1.000 }; pose_eye[5][0] = { 0.139,-0.990,-0.011 }; pose_eye[5][1] = { 0.990,0.139,0.001 }; pose_eye[5][2] = { 0.001,-0.011,1.000 };
pose_eye[6][0] = { 0.136746, -0.990563, -0.00926168 }; pose_eye[6][1] = { 0.990606, 0.136747, 0.000517805 }; pose_eye[6][2] = { 0.000753588, -0.00924548, 0.999957 };
for (int i = 0; i < 6; i++) for (int i = 0; i < 7; i++)
{ {
cv::Point3d rtPt; cv::Point3d rtPt;
pointRT_2(R, T, pts_eye[i], rtPt); //RT前后的点 pointRT_2(R, T, verify_pts_eye[i], rtPt); //RT前后的点
std::vector<cv::Point3d> dirVectors_eye = pose_eye[i]; std::vector<cv::Point3d> dirVectors_eye = pose_eye[i];
//dirVectors_eye[0] = { -dirVectors_eye[0].x, -dirVectors_eye[0].y, -dirVectors_eye[0].z };
dirVectors_eye[1] = { -dirVectors_eye[1].x, -dirVectors_eye[1].y, -dirVectors_eye[1].z }; dirVectors_eye[1] = { -dirVectors_eye[1].x, -dirVectors_eye[1].y, -dirVectors_eye[1].z };
dirVectors_eye[2] = { -dirVectors_eye[2].x, -dirVectors_eye[2].y, -dirVectors_eye[2].z }; dirVectors_eye[2] = { -dirVectors_eye[2].x, -dirVectors_eye[2].y, -dirVectors_eye[2].z };
std::vector<cv::Point3d> dirVectors_robot; std::vector<cv::Point3d> dirVectors_robot;
@ -613,7 +624,7 @@ int main()
groundCalibPara.invRMatrix[i] = groundCalibPara.planeCalib[i]; groundCalibPara.invRMatrix[i] = groundCalibPara.planeCalib[i];
char calibFile[250]; char calibFile[250];
sprintf_s(calibFile, "%sground_calib_para.txt", dataPath[grp]); sprintf_s(calibFile, "%sground_calib_para.txt", dataPath[grp]);
//groundCalibPara = _readCalibPara(calibFile); groundCalibPara = _readCalibPara(calibFile);
for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
{ {