619 lines
18 KiB
C++
619 lines
18 KiB
C++
#include <vector>
|
||
#include "SG_baseDataType.h"
|
||
#include "SG_baseAlgo_Export.h"
|
||
#include "BQ_assemblyPosition_Export.h"
|
||
#include <opencv2/opencv.hpp>
|
||
#include <limits>
|
||
|
||
//version 1.0.0 : base version release to customer
|
||
std::string m_strVersion = "1.0.0";
|
||
const char* wd_BQAssemblyPositionVersion(void)
|
||
{
|
||
return m_strVersion.c_str();
|
||
}
|
||
|
||
//针对孔板计算一个平面调平参数：简化了算法，提取扫描线中没有孔的扫描线进行拟合
|
||
//数据输入中可以有一个地平面和参考调平平面，以最高的平面进行调平
|
||
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
|
||
SSG_planeCalibPara sx_BQ_getHoleBaseCalibPara(
|
||
std::vector< std::vector<SVzNL3DPosition>>& scanLines)
|
||
{
|
||
return sg_getHolePlaneCalibPara(scanLines);
|
||
}
|
||
|
||
//相机姿态调平，并去除地面
|
||
void sx_BQ_lineDataR(
|
||
std::vector< SVzNL3DPosition>& a_line,
|
||
const double* camPoseR,
|
||
double groundH)
|
||
{
|
||
lineDataRT_vector(a_line, camPoseR, groundH);
|
||
}
|
||
|
||
int _counterLinePtNum(std::vector<SVzNL3DPosition>& lineData)
|
||
{
|
||
int ptNum = 0;
|
||
for (int i = 0, i_max = (int)lineData.size(); i < i_max; i++)
|
||
{
|
||
if ((abs(lineData[i].pt3D.z) > 1e-4) ||
|
||
(abs(lineData[i].pt3D.x > 1e-4)) ||
|
||
(abs(lineData[i].pt3D.y > 1e-4)))
|
||
ptNum++;
|
||
}
|
||
return ptNum;
|
||
}
|
||
|
||
bool compareByAngle(const SWD_polarPt& a, const SWD_polarPt& b) {
|
||
return a.angle < b.angle;
|
||
}
|
||
|
||
SSX_BQAssemblyInfo sx_BQ_computeAssemblyInfoFrom3D(
|
||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||
const SSG_cornerParam cornerPara,
|
||
const SSG_outlierFilterParam filterParam,
|
||
SSG_treeGrowParam growParam,
|
||
SSG_planeCalibPara groundCalibPara,
|
||
SSX_BQAssemblyPara assemblyParam,
|
||
int* errCode)
|
||
{
|
||
*errCode = 0;
|
||
SSX_BQAssemblyInfo assemblyPose;
|
||
memset(&assemblyPose, 0, sizeof(SSX_BQAssemblyInfo));
|
||
int lineNum = (int)scanLines.size();
|
||
if (lineNum == 0)
|
||
{
|
||
*errCode = SG_ERR_3D_DATA_NULL;
|
||
return assemblyPose;
|
||
}
|
||
|
||
std::vector<std::vector<int>> groundMask;
|
||
for (int line = 0, line_max = (int)scanLines.size(); line < line_max; line++)
|
||
{
|
||
//调平，去除地面
|
||
std::vector<SVzNL3DPosition>& a_line = scanLines[line];
|
||
std::vector<int> a_lineMask;
|
||
a_lineMask.resize(a_line.size());
|
||
for (int i = 0; i < a_line.size(); i++)
|
||
{
|
||
if ((line == 1000) && (i == 1439))
|
||
int kkk = 1;
|
||
a_line[i].nPointIdx = i;
|
||
a_lineMask[i] = 0;
|
||
SVzNL3DPoint& a_pt = a_line[i].pt3D;
|
||
if (a_pt.z < 1e-4)
|
||
continue;
|
||
double x = a_pt.x * groundCalibPara.planeCalib[0] + a_pt.y * groundCalibPara.planeCalib[1] + a_pt.z * groundCalibPara.planeCalib[2];
|
||
double y = a_pt.x * groundCalibPara.planeCalib[3] + a_pt.y * groundCalibPara.planeCalib[4] + a_pt.z * groundCalibPara.planeCalib[5];
|
||
double z = a_pt.x * groundCalibPara.planeCalib[6] + a_pt.y * groundCalibPara.planeCalib[7] + a_pt.z * groundCalibPara.planeCalib[8];
|
||
if ((groundCalibPara.planeHeight > 0) && (z > (groundCalibPara.planeHeight-5))) //生成地面Mask
|
||
{
|
||
a_lineMask[i] = 1; // a_line[i].nPointIdx = -1; //mark
|
||
}
|
||
a_pt.x = x;
|
||
a_pt.y = y;
|
||
a_pt.z = z;
|
||
a_line[i].pt3D = a_pt;
|
||
}
|
||
groundMask.push_back(a_lineMask);
|
||
}
|
||
|
||
//将开始和结束的空白扫描线去除，获得扫描边界
|
||
int validStartLine = -1;
|
||
for (int i = 0; i < lineNum; i++)
|
||
{
|
||
int linePtNum = _counterLinePtNum(scanLines[i]);
|
||
if (linePtNum > 0)
|
||
{
|
||
validStartLine = i;
|
||
break;
|
||
}
|
||
}
|
||
int validEndLine = -1;
|
||
for (int i = lineNum - 1; i >= 0; i--)
|
||
{
|
||
int linePtNum = _counterLinePtNum(scanLines[i]);
|
||
if (linePtNum > 0)
|
||
{
|
||
validEndLine = i;
|
||
break;
|
||
}
|
||
}
|
||
if ((validStartLine < 0) || (validEndLine < 0))
|
||
{
|
||
*errCode = SG_ERR_3D_DATA_NULL;
|
||
return assemblyPose;
|
||
}
|
||
|
||
int linePtNum = (int)scanLines[0].size();
|
||
bool isGridData = true;
|
||
|
||
//自适应各种旋转角度
|
||
//垂直跳变特征提取
|
||
double contourWin = 40.0; //地面附近的拐点视为端点
|
||
std::vector<std::vector<SSG_basicFeature1D>> jumpFeatures_v_raw;
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
if (line == 1000)
|
||
int kkk = 1;
|
||
|
||
std::vector<SVzNL3DPosition>& lineData = scanLines[line];
|
||
if (linePtNum != (int)lineData.size())
|
||
isGridData = false;
|
||
|
||
//滤波，滤除异常点
|
||
sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam);
|
||
|
||
std::vector<SSG_basicFeature1D> line_features;
|
||
int dataSize = (int)lineData.size();
|
||
/// 提取激光线上的拐点特征，地面端点附近的拐点视为合格拐点
|
||
sg_getLineContourCornerFeature_groundMask_BQ(
|
||
lineData,
|
||
groundMask[line],
|
||
line,
|
||
contourWin, //ground边缘范围
|
||
cornerPara, //scale通常取bagH的1/4
|
||
line_features);
|
||
jumpFeatures_v_raw.push_back(line_features);
|
||
}
|
||
|
||
if (false == isGridData)//数据不是网格格式
|
||
{
|
||
*errCode = SG_ERR_NOT_GRID_FORMAT;
|
||
return assemblyPose;
|
||
}
|
||
|
||
//生成水平扫描
|
||
std::vector<std::vector<SSG_basicFeature1D>> jumpFeatures_h_raw;
|
||
std::vector<std::vector<SVzNL3DPosition>> hLines_raw;
|
||
std::vector<std::vector<int>> groundMask_hLines;
|
||
hLines_raw.resize(linePtNum);
|
||
int lineNum_h_raw = (int)hLines_raw.size();
|
||
#if 0
|
||
hLines_raw.resize(linePtNum);
|
||
groundMask_hLines.resize(linePtNum);
|
||
for (int i = 0; i < linePtNum; i++)
|
||
{
|
||
hLines_raw[i].resize(lineNum);
|
||
groundMask_hLines[i].resize(lineNum);
|
||
}
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
for (int j = 0; j < linePtNum; j++)
|
||
{
|
||
scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
|
||
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;
|
||
groundMask_hLines[j][line] = groundMask[line][j];
|
||
}
|
||
}
|
||
//水平arc特征提取
|
||
for (int line = 0; line < lineNum_h_raw; line++)
|
||
{
|
||
if (line == 1906)
|
||
int kkk = 1;
|
||
std::vector<SVzNL3DPosition>& lineData = hLines_raw[line];
|
||
//滤波，滤除异常点
|
||
int ptNum = (int)lineData.size();
|
||
sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], ptNum, filterParam);
|
||
|
||
std::vector<SSG_basicFeature1D> line_features;
|
||
int dataSize = (int)lineData.size();
|
||
|
||
sg_getLineContourCornerFeature_groundMask_BQ(
|
||
hLines_raw[line],
|
||
groundMask_hLines[line],
|
||
line,
|
||
contourWin, //ground边缘范围
|
||
cornerPara, //scale通常取bagH的1/4
|
||
line_features);
|
||
jumpFeatures_h_raw.push_back(line_features);
|
||
}
|
||
#endif
|
||
//特征生长，用于滤除噪点
|
||
//使用聚类法代替水平和垂直特征生长
|
||
|
||
std::vector<std::vector<SSG_featureClusteringInfo>> featureInfoMask;
|
||
std::vector<std::vector<SVzNL3DPoint>> feature3DInfo;
|
||
featureInfoMask.resize(lineNum);
|
||
feature3DInfo.resize(lineNum);
|
||
for (int i = 0; i < lineNum; i++)
|
||
{
|
||
featureInfoMask[i].resize(linePtNum);
|
||
feature3DInfo[i].resize(linePtNum);
|
||
}
|
||
//生成Mask
|
||
double meanX = 0, meanY = 0, meanZ = 0;
|
||
int objPtSize = 0;
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
int featureNum = jumpFeatures_v_raw[line].size();
|
||
for (int j = 0; j < featureNum; j++)
|
||
{
|
||
SVzNL2DPoint& a_pos = jumpFeatures_v_raw[line][j].jumpPos2D;
|
||
SSG_featureClusteringInfo a_mask;
|
||
memset(&a_mask, 0, sizeof(SSG_featureClusteringInfo));
|
||
a_mask.featurType = 1;
|
||
a_mask.lineIdx = a_pos.x;
|
||
a_mask.ptIdx = a_pos.y;
|
||
featureInfoMask[a_pos.x][a_pos.y] = a_mask;
|
||
feature3DInfo[a_pos.x][a_pos.y] = scanLines[a_pos.x][a_pos.y].pt3D;
|
||
meanX += scanLines[a_pos.x][a_pos.y].pt3D.x;
|
||
meanY += scanLines[a_pos.x][a_pos.y].pt3D.y;
|
||
meanZ += scanLines[a_pos.x][a_pos.y].pt3D.z;
|
||
objPtSize++;
|
||
}
|
||
}
|
||
#if 0
|
||
for (int line = 0; line < lineNum_h_raw; line++)
|
||
{
|
||
int featureNum = jumpFeatures_h_raw[line].size();
|
||
for (int j = 0; j < featureNum; j++)
|
||
{
|
||
SVzNL2DPoint& a_pos = jumpFeatures_v_raw[line][j].jumpPos2D;
|
||
if (featureInfoMask[a_pos.y][a_pos.x].featurType == 0)
|
||
{
|
||
SSG_featureClusteringInfo a_mask;
|
||
memset(&a_mask, 0, sizeof(SSG_featureClusteringInfo));
|
||
a_mask.featurType = 1;
|
||
a_mask.lineIdx = a_pos.y;
|
||
a_mask.ptIdx = a_pos.x;
|
||
featureInfoMask[a_pos.y][a_pos.x] = a_mask;
|
||
feature3DInfo[a_pos.y][a_pos.x] = scanLines[a_pos.y][a_pos.x].pt3D;
|
||
}
|
||
}
|
||
}
|
||
#endif
|
||
|
||
#if 0
|
||
//聚类
|
||
//采用迭代思想，回归思路进行高效聚类
|
||
std::vector<std::vector< SVzNL2DPoint>> clusters; //只记录位置
|
||
std::vector<SWD_clustersInfo> clustersInfo;
|
||
int clusterID = 1;
|
||
int clusterCheckWin = 30;
|
||
for (int y = 0; y < linePtNum; y++)
|
||
{
|
||
for (int x = 0; x < lineNum; x++)
|
||
{
|
||
SSG_featureClusteringInfo& a_featureInfo = featureInfoMask[x][y];
|
||
if ((0 == a_featureInfo.featurType) || (a_featureInfo.clusterID > 0)) //非特征或已经处理
|
||
continue;
|
||
|
||
SVzNL3DPoint& a_feature3DValue = feature3DInfo[x][y];
|
||
SVzNL3DRangeD a_clusterRoi;
|
||
a_clusterRoi.xRange.min = a_feature3DValue.x;
|
||
a_clusterRoi.xRange.max = a_feature3DValue.x;
|
||
a_clusterRoi.yRange.min = a_feature3DValue.y;
|
||
a_clusterRoi.yRange.max = a_feature3DValue.y;
|
||
a_clusterRoi.zRange.min = a_feature3DValue.z;
|
||
a_clusterRoi.zRange.max = a_feature3DValue.z;
|
||
|
||
SVzNL2DPoint a_seedPos = { x, y };
|
||
std::vector< SVzNL2DPoint> a_cluster;
|
||
a_cluster.push_back(a_seedPos);
|
||
wd_gridPointClustering(
|
||
featureInfoMask,//int，记录特征标记和clusterID，附加一个flag
|
||
feature3DInfo,//double,记录坐标信息
|
||
clusterCheckWin, //搜索窗口
|
||
growParam,//聚类条件
|
||
clusterID, //当前Cluster的ID
|
||
a_cluster, //result
|
||
a_clusterRoi
|
||
);
|
||
clusters.push_back(a_cluster);
|
||
SWD_clustersInfo a_info;
|
||
a_info.clusterIdx = clusterID;
|
||
a_info.ptSize = (int)a_cluster.size();
|
||
a_info.roi3D = a_clusterRoi;
|
||
clustersInfo.push_back(a_info);
|
||
clusterID++;
|
||
}
|
||
}
|
||
//取最大的聚类为边界
|
||
int clusterSize = clusters.size();
|
||
if(clusterSize == 0)
|
||
{
|
||
*errCode = SX_ERR_ZERO_OBJ;
|
||
return assemblyPose;
|
||
}
|
||
|
||
int objPtSize = clusters[0].size();
|
||
int objClusterID = 0;
|
||
for (int i = 1; i < clusterSize; i++)
|
||
{
|
||
if (clusters[i].size() > objPtSize)
|
||
{
|
||
objPtSize = (int)clusters[i].size();
|
||
objClusterID = i;
|
||
}
|
||
}
|
||
if (objPtSize == 0)
|
||
{
|
||
*errCode = SX_ERR_ZERO_OBJ;
|
||
return assemblyPose;
|
||
}
|
||
|
||
double meanX = 0, meanY = 0, meanZ = 0;
|
||
for (int i = 0; i < objPtSize; i++)
|
||
{
|
||
int objLineIdx = clusters[objClusterID][i].x;
|
||
int objPtIdx = clusters[objClusterID][i].y;
|
||
scanLines[objLineIdx][objPtIdx].nPointIdx = 1;
|
||
meanX += scanLines[objLineIdx][objPtIdx].pt3D.x;
|
||
meanY += scanLines[objLineIdx][objPtIdx].pt3D.y;
|
||
meanZ += scanLines[objLineIdx][objPtIdx].pt3D.z;
|
||
}
|
||
#endif
|
||
meanX = meanX / objPtSize;
|
||
meanY = meanY / objPtSize;
|
||
meanZ = meanZ / objPtSize;
|
||
assemblyPose.O.x = meanX;
|
||
assemblyPose.O.y = meanY;
|
||
assemblyPose.O.z = meanZ;
|
||
|
||
#if 0
|
||
std::vector<SWD_polarPt> polarPoints;
|
||
for (int i = 0, i_max = (int)v_trees.size(); i < i_max; i++)
|
||
{
|
||
SSG_featureTree* a_vTree = &v_trees[i];
|
||
//在原始点云上标记，同时有Mask上标记
|
||
for (int j = 0, j_max = (int)a_vTree->treeNodes.size(); j < j_max; j++)
|
||
{
|
||
int lineIdx = a_vTree->treeNodes[j].jumpPos2D.x;
|
||
int ptIdx = a_vTree->treeNodes[j].jumpPos2D.y;
|
||
if (scanLines[lineIdx][ptIdx].nPointIdx >= 0)
|
||
{
|
||
SWD_polarPt a_polarPt;
|
||
a_polarPt.lineIdx = lineIdx;
|
||
a_polarPt.ptIdx = ptIdx;
|
||
a_polarPt.R = 0;
|
||
a_polarPt.angle = 0;
|
||
a_polarPt.x = scanLines[lineIdx][ptIdx].pt3D.x;
|
||
a_polarPt.y = scanLines[lineIdx][ptIdx].pt3D.y;
|
||
a_polarPt.z = scanLines[lineIdx][ptIdx].pt3D.z;
|
||
polarPoints.push_back(a_polarPt);
|
||
scanLines[lineIdx][ptIdx].nPointIdx = -1;
|
||
}
|
||
}
|
||
}
|
||
for (int i = 0, i_max = (int)h_trees.size(); i < i_max; i++)
|
||
{
|
||
SSG_featureTree* a_hTree = &h_trees[i];
|
||
//在原始点云上标记，同时有Mask上标记
|
||
for (int j = 0, j_max = (int)a_hTree->treeNodes.size(); j < j_max; j++)
|
||
{
|
||
int lineIdx = a_hTree->treeNodes[j].jumpPos2D.y;
|
||
int ptIdx = a_hTree->treeNodes[j].jumpPos2D.x;
|
||
if (scanLines[lineIdx][ptIdx].nPointIdx >= 0)
|
||
{
|
||
SWD_polarPt a_polarPt;
|
||
a_polarPt.lineIdx = lineIdx;
|
||
a_polarPt.ptIdx = ptIdx;
|
||
a_polarPt.cptIndex = -1;
|
||
a_polarPt.R = 0;
|
||
a_polarPt.angle = 0;
|
||
a_polarPt.x = scanLines[lineIdx][ptIdx].pt3D.x;
|
||
a_polarPt.y = scanLines[lineIdx][ptIdx].pt3D.y;
|
||
a_polarPt.z = scanLines[lineIdx][ptIdx].pt3D.z;
|
||
polarPoints.push_back(a_polarPt);
|
||
scanLines[lineIdx][ptIdx].nPointIdx = -1;
|
||
}
|
||
}
|
||
}
|
||
|
||
//计算几何中心
|
||
int contourPtSize = (int)polarPoints.size();
|
||
if (contourPtSize == 0)
|
||
{
|
||
*errCode = SX_ERR_ZERO_CONTOUR_PT;
|
||
return assemblyPose;
|
||
}
|
||
double center_x = 0;
|
||
double center_y = 0;
|
||
for (int pi = 0; pi < contourPtSize; pi++)
|
||
{
|
||
center_x += polarPoints[pi].x;
|
||
center_y += polarPoints[pi].y;
|
||
}
|
||
center_x = center_x / (double)contourPtSize;
|
||
center_y = center_y / (double)contourPtSize;
|
||
//计算极坐标的R和Theta
|
||
for (int pi = 0; pi < contourPtSize; pi++)
|
||
{
|
||
double angle = atan2(polarPoints[pi].y - center_y, polarPoints[pi].x - center_x);
|
||
angle = (angle / PI) * 180 + 180.0;
|
||
double R = sqrt(pow(polarPoints[pi].y - center_y, 2) + pow(polarPoints[pi].x - center_x, 2));
|
||
polarPoints[pi].R = R;
|
||
polarPoints[pi].angle = angle;
|
||
}
|
||
//按角度大小排序
|
||
std::sort(polarPoints.begin(), polarPoints.end(), compareByAngle);
|
||
for (int pi = 0; pi < contourPtSize; pi++)
|
||
polarPoints[pi].cptIndex = pi; // index
|
||
|
||
//提取R极值点
|
||
double minR = -1, maxR = -1; //计算最小和最大的R，用以区分有没有分支。minR和maxR相差小时，为圆形或8角形，没有分支
|
||
int minRPos = -1;
|
||
std::vector<SWD_polarPt> polarRPeakPts;
|
||
int winSize = contourPtSize / 36; //+-10度范围
|
||
if (winSize < 5)
|
||
winSize = 5;
|
||
for (int pi = 0; pi < contourPtSize; pi++)
|
||
{
|
||
double currR = polarPoints[pi].R;
|
||
if (minR < 0)
|
||
{
|
||
minR = currR;
|
||
maxR = currR;
|
||
minRPos = pi;
|
||
}
|
||
else
|
||
{
|
||
minRPos = minR > currR ? pi : minRPos;
|
||
minR = minR > currR ? currR : minR;
|
||
maxR = maxR < currR ? currR : maxR;
|
||
}
|
||
bool isPeak = true;
|
||
for (int k = -winSize; k <= winSize; k++)
|
||
{
|
||
int idx = (pi + k + contourPtSize) % contourPtSize; //筒形结构
|
||
if (polarPoints[idx].R > currR)
|
||
{
|
||
isPeak = false;
|
||
break;
|
||
}
|
||
}
|
||
if (true == isPeak)
|
||
polarRPeakPts.push_back(polarPoints[pi]);
|
||
}
|
||
|
||
double ratio_MaxMin = maxR / minR;
|
||
bool hasBranch = ratio_MaxMin < 1.25 ? false : true;
|
||
|
||
std::vector<SWD_polarPt> validPolarRPeakPts;
|
||
std::vector<SWD_polarPeakInfo> polarPeakInfo;
|
||
int pkId = 0;
|
||
//过滤圆弧段的极值：由于重心偏移，圆弧段也会形成极值。根据极值两边L=直线段长度构成的张角判断
|
||
double arcAngleChkLen = 100; //检测圆弧张角的尺度
|
||
for (int i = 0, i_max = (int)polarRPeakPts.size(); i < i_max; i++)
|
||
{
|
||
int ptidx = polarRPeakPts[i].cptIndex;
|
||
double px, py, pz;
|
||
px = polarRPeakPts[i].x;
|
||
py = polarRPeakPts[i].y;
|
||
|
||
int LL1 = -1;
|
||
int halfLL1 = -1;
|
||
for (int j = ptidx - 1; j > -contourPtSize; j--)
|
||
{
|
||
int idx = (j + contourPtSize) % contourPtSize; //筒形结构
|
||
double cx = polarPoints[idx].x;
|
||
double cy = polarPoints[idx].y;
|
||
double len = sqrt(pow(px - cx, 2) + pow(py - cy, 2));
|
||
if (len < arcAngleChkLen)
|
||
halfLL1 = idx;
|
||
if (len > (assemblyParam.lineLen))
|
||
{
|
||
LL1 = idx;
|
||
break;
|
||
}
|
||
}
|
||
|
||
int LL2 = -1;
|
||
int halfLL2 = -1;
|
||
for (int j = ptidx + 1; j < contourPtSize * 2; j++)
|
||
{
|
||
int idx = j % contourPtSize; //筒形结构
|
||
double cx = polarPoints[idx].x;
|
||
double cy = polarPoints[idx].y;
|
||
double len = sqrt(pow(px - cx, 2) + pow(py - cy, 2));
|
||
if (len < arcAngleChkLen)
|
||
halfLL2 = idx;
|
||
if (len > (assemblyParam.lineLen))
|
||
{
|
||
LL2 = idx;
|
||
break;
|
||
}
|
||
}
|
||
|
||
if ((LL1 >= 0) && (LL2 >= 0))
|
||
{
|
||
double len1 = sqrt(pow(px - polarPoints[halfLL1].x, 2) + pow(py - polarPoints[halfLL1].y, 2));
|
||
double len2 = sqrt(pow(px - polarPoints[halfLL2].x, 2) + pow(py - polarPoints[halfLL2].y, 2));
|
||
double len3 = sqrt(pow(polarPoints[halfLL1].x - polarPoints[halfLL2].x, 2) +
|
||
pow(polarPoints[halfLL1].y - polarPoints[halfLL2].y, 2));
|
||
double cosTheta = (len1 * len1 + len2 * len2 - len3 * len3) / (2 * len1 * len2);
|
||
double theta = acos(cosTheta) * 180.0 / PI;
|
||
if (theta < 150)
|
||
{
|
||
SWD_polarPeakInfo a_pkInfo;
|
||
a_pkInfo.cptIndex = ptidx;
|
||
a_pkInfo.L1_ptIndex = LL1;
|
||
a_pkInfo.L2_ptIndex = LL2;
|
||
a_pkInfo.cornerAngle = theta;
|
||
a_pkInfo.cornerDir = 0;
|
||
polarRPeakPts[i].cptIndex = ptidx;
|
||
polarRPeakPts[i].pkId = pkId;
|
||
pkId++;
|
||
validPolarRPeakPts.push_back(polarRPeakPts[i]);
|
||
polarPeakInfo.push_back(a_pkInfo);
|
||
}
|
||
}
|
||
}
|
||
//处理矩形工件
|
||
if (validPolarRPeakPts.size() != 4)
|
||
{
|
||
*errCode = SX_ERR_INVLID_RPEAK_NUM;
|
||
return assemblyPose;
|
||
}
|
||
|
||
//取长边作X轴，短边作Y轴，法向为Z轴，中心点为O点
|
||
std::vector<SVzNL3DPoint> sidePts_1;
|
||
std::vector<SVzNL3DPoint> sidePts_2;
|
||
std::vector<SVzNL3DPoint> sidePts_3;
|
||
std::vector<SVzNL3DPoint> sidePts_4;
|
||
#endif
|
||
return assemblyPose;
|
||
}
|
||
|
||
//根据Mark计算工件位置和姿态
|
||
SSX_BQAssemblyInfo sx_BQ_computeAssemblyInfoFromMark(
|
||
SSX_BQAssemblyInfo& originPos,
|
||
std::vector<SVzNL3DPoint>& originMark,
|
||
std::vector<SVzNL3DPoint>& currMark,
|
||
int* errCode)
|
||
{
|
||
*errCode = 0;
|
||
SSX_BQAssemblyInfo resultPos;
|
||
memset(&resultPos, 0, sizeof(SSX_BQAssemblyInfo));
|
||
if ((originMark.size() < 3) || (currMark.size() < 3) || (originMark.size() != currMark.size()))
|
||
{
|
||
*errCode = SX_ERR_INVLID_MARK_NUM;
|
||
return resultPos;
|
||
}
|
||
|
||
cv::Mat R, T; //旋转平移
|
||
cv::Point3d C_origin, C_curr; //质心
|
||
|
||
std::vector<cv::Point3d> originMarkPos;
|
||
originMarkPos.resize(originMark.size());
|
||
std::vector<cv::Point3d> currMarkPos;
|
||
currMarkPos.resize(originMark.size());
|
||
for (int i = 0; i < (int)originMark.size(); i++)
|
||
{
|
||
originMarkPos[i] = { originMark[i].x, originMark[i].y, originMark[i].z };
|
||
currMarkPos[i] = { currMark[i].x, currMark[i].y, currMark[i].z};
|
||
}
|
||
caculateRT(originMarkPos, currMarkPos, R, T, C_origin, C_curr);
|
||
std::vector<cv::Point3d> originMarkPos_RT;
|
||
originMarkPos_RT.resize(originMarkPos.size());
|
||
for (int i = 0; i < (int)originMark.size(); i++)
|
||
{
|
||
cv::Point3d RT_pt;
|
||
pointRT(R, T, C_origin, C_curr, originMarkPos[i], RT_pt);
|
||
originMarkPos_RT[i] = RT_pt;
|
||
}
|
||
|
||
// 5. 推算Pn的坐标
|
||
cv::Point3d O = { originPos.O.x, originPos.O.y, originPos.O.z};
|
||
cv::Point3d X = { originPos.X.x, originPos.X.y, originPos.X.z };
|
||
cv::Point3d Y = { originPos.Y.x, originPos.Y.y, originPos.Y.z };
|
||
cv::Point3d Z = { originPos.Z.x, originPos.Z.y, originPos.Z.z };
|
||
|
||
cv::Point3d RO, RX, RY, RZ;
|
||
pointRT(R, T, C_origin, C_curr, O, RO);
|
||
pointRT(R, T, C_origin, C_curr, X, RX);
|
||
pointRT(R, T, C_origin, C_curr, Y, RY);
|
||
pointRT(R, T, C_origin, C_curr, Z, RZ);
|
||
resultPos.O = { RO.x, RO.y, RO.z };
|
||
resultPos.X = { RX.x, RX.y, RX.z };
|
||
resultPos.Y = { RY.x, RY.y, RY.z };
|
||
resultPos.Z = { RZ.x, RZ.y, RZ.z };
|
||
|
||
return resultPos;
|
||
}
|
||
|
||
|
||
|