1982 lines
60 KiB
C++
1982 lines
60 KiB
C++
#include <vector>
|
||
#include "SG_baseDataType.h"
|
||
#include "SG_baseAlgo_Export.h"
|
||
#include "rodAndBarDetection_Export.h"
|
||
#include <opencv2/opencv.hpp>
|
||
#include <limits>
|
||
|
||
//version 1.0.0 : base version release to customer
|
||
//version 1.1.0 : 添加了地面调平和棒材定位
|
||
//version 1.1.1 : 初始发布给客户的版本
|
||
//version 1.2.0 : 配天螺杆测量增加了定位盘中心测量功能
|
||
//version 1.2.1 : 增加了定位盘中心完整姿态输出
|
||
//version 1.2.2 : 调整了定位盘中心姿态定义：法向（Z向）：向右；Y：向下
|
||
//version 1.2.3 : 根据定向盘轮廓的直线段确定向右的向量
|
||
//version 1.2.4 : 根据定向盘左上点和右下点确定姿态向量
|
||
std::string m_strVersion = "1.2.4";
|
||
const char* wd_rodAndBarDetectionVersion(void)
|
||
{
|
||
return m_strVersion.c_str();
|
||
}
|
||
|
||
//计算一个平面调平参数。
|
||
//数据输入中可以有一个地平面和参考调平平面，以最高的平面进行调平
|
||
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
|
||
SSG_planeCalibPara sx_rodPosition_getBaseCalibPara(
|
||
std::vector< std::vector<SVzNL3DPosition>>& scanLines)
|
||
{
|
||
return sg_getPlaneCalibPara2(scanLines);
|
||
}
|
||
|
||
//相机姿态调平，并去除地面
|
||
void sx_rodPosition_lineDataR(
|
||
std::vector< SVzNL3DPosition>& a_line,
|
||
const double* camPoseR,
|
||
double groundH)
|
||
{
|
||
lineDataRT_vector(a_line, camPoseR, groundH);
|
||
}
|
||
|
||
SVzNL3DPoint _translatePoint(SVzNL3DPoint point, const double rMatrix[9])
|
||
{
|
||
SVzNL3DPoint result;
|
||
double x = point.x * rMatrix[0] + point.y * rMatrix[1] + point.z * rMatrix[2];
|
||
double y = point.x * rMatrix[3] + point.y * rMatrix[4] + point.z * rMatrix[5];
|
||
double z = point.x * rMatrix[6] + point.y * rMatrix[7] + point.z * rMatrix[8];
|
||
result.x = x;
|
||
result.y = y;
|
||
result.z = z;
|
||
return result;
|
||
}
|
||
|
||
|
||
SVzNL3DPoint getArcPeak(
|
||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||
SWD_segFeature & a_arcFeature,
|
||
SVzNL2DPoint& arcPos)
|
||
{
|
||
SVzNL3DPoint arcPeak = scanLines[a_arcFeature.lineIdx][a_arcFeature.startPtIdx].pt3D;
|
||
for (int i = a_arcFeature.startPtIdx+1; i <= a_arcFeature.endPtIdx; i++)
|
||
{
|
||
if (scanLines[a_arcFeature.lineIdx][i].pt3D.z > 1e-4) //跳开空点
|
||
{
|
||
if (arcPeak.z > scanLines[a_arcFeature.lineIdx][i].pt3D.z)
|
||
{
|
||
arcPeak = scanLines[a_arcFeature.lineIdx][i].pt3D;
|
||
arcPos = { a_arcFeature.lineIdx , i };
|
||
}
|
||
}
|
||
}
|
||
return arcPeak;
|
||
}
|
||
|
||
SVzNL3DPoint getArcPeak_parabolaFitting(
|
||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||
SWD_segFeature& a_arcFeature,
|
||
SVzNL2DPoint& arcPos)
|
||
{
|
||
std::vector<cv::Point2d> points;
|
||
for (int i = a_arcFeature.startPtIdx + 1; i <= a_arcFeature.endPtIdx; i++)
|
||
{
|
||
if (scanLines[a_arcFeature.lineIdx][i].pt3D.z > 1e-4) //跳开空点
|
||
{
|
||
cv::Point2d a_pt2D;
|
||
if (scanLines[a_arcFeature.lineIdx][i].pt3D.z > 1e-4)
|
||
{
|
||
a_pt2D.x = scanLines[a_arcFeature.lineIdx][i].pt3D.y;
|
||
a_pt2D.y = scanLines[a_arcFeature.lineIdx][i].pt3D.z;
|
||
points.push_back(a_pt2D);
|
||
}
|
||
}
|
||
}
|
||
double a, b, c, mse, max_err;
|
||
//抛物线最小二乘拟合 y = ax ^ 2 + bx + c
|
||
bool result = leastSquareParabolaFitEigen(
|
||
points,
|
||
a, b, c,
|
||
mse, max_err);
|
||
double yP = -b / (2 * a);
|
||
//寻找与yP最近的点作为Peak点
|
||
SVzNL3DPoint arcPeak;
|
||
double minDist = -1;
|
||
for (int i = a_arcFeature.startPtIdx + 1; i <= a_arcFeature.endPtIdx; i++)
|
||
{
|
||
if (scanLines[a_arcFeature.lineIdx][i].pt3D.z > 1e-4) //跳开空点
|
||
{
|
||
double dist = abs(scanLines[a_arcFeature.lineIdx][i].pt3D.y - yP);
|
||
if (minDist < 0)
|
||
{
|
||
minDist = dist;
|
||
arcPeak = scanLines[a_arcFeature.lineIdx][i].pt3D;
|
||
arcPos = { a_arcFeature.lineIdx , i };
|
||
}
|
||
else
|
||
{
|
||
if(minDist > dist)
|
||
{
|
||
minDist = dist;
|
||
arcPeak = scanLines[a_arcFeature.lineIdx][i].pt3D;
|
||
arcPos = { a_arcFeature.lineIdx , i };
|
||
}
|
||
}
|
||
}
|
||
}
|
||
return arcPeak;
|
||
}
|
||
|
||
//投影，提取ROI内的数据
|
||
void xoyROIProjection(
|
||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||
const double* rtMatrix,
|
||
SSG_ROIRectD& roi_xoy,
|
||
std::vector<SVzNL3DPoint>& projectPoints
|
||
)
|
||
{
|
||
int lineNum = (int)scanLines.size();
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
std::vector<SVzNL3DPosition>& a_line = scanLines[line];
|
||
int ptNum = (int)a_line.size();
|
||
for (int i = 0; i < (int)a_line.size(); i++)
|
||
{
|
||
SVzNL3DPoint a_pt = a_line[i].pt3D;
|
||
if (a_pt.z < 1e-4)
|
||
continue;
|
||
double x = a_pt.x * rtMatrix[0] + a_pt.y * rtMatrix[1] + a_pt.z * rtMatrix[2];
|
||
double y = a_pt.x * rtMatrix[3] + a_pt.y * rtMatrix[4] + a_pt.z * rtMatrix[5];
|
||
double z = a_pt.x * rtMatrix[6] + a_pt.y * rtMatrix[7] + a_pt.z * rtMatrix[8];
|
||
if ((x >= roi_xoy.left) && (x <= roi_xoy.right) &&
|
||
(y >= roi_xoy.top) && (y <= roi_xoy.bottom))
|
||
{
|
||
a_pt.x = x;
|
||
a_pt.y = y;
|
||
a_pt.z = z;
|
||
projectPoints.push_back(a_pt);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
SVzNLRangeD getZRange(std::vector<SVzNL3DPoint>& projectPoints)
|
||
{
|
||
int ptNum = (int)projectPoints.size();
|
||
SVzNLRangeD zRange;
|
||
zRange.min = DBL_MAX;
|
||
zRange.max = DBL_MIN;
|
||
for (int i = 0; i < ptNum; i++)
|
||
{
|
||
zRange.min = zRange.min > projectPoints[i].z ? projectPoints[i].z : zRange.min;
|
||
zRange.max = zRange.max < projectPoints[i].z ? projectPoints[i].z : zRange.max;
|
||
}
|
||
return zRange;
|
||
}
|
||
|
||
void zCutPointClouds(
|
||
std::vector<SVzNL3DPoint>& projectPoints,
|
||
SVzNLRangeD& zRange,
|
||
std::vector<SVzNL3DPoint>& cutLayerPoints)
|
||
{
|
||
int ptNum = (int)projectPoints.size();
|
||
for (int i = 0; i < ptNum; i++)
|
||
{
|
||
if ((projectPoints[i].z >= zRange.min) && (projectPoints[i].z <= zRange.max))
|
||
cutLayerPoints.push_back(projectPoints[i]);
|
||
}
|
||
}
|
||
|
||
SVzNL3DPoint getXoYCentroid(std::vector<SVzNL3DPoint>& points)
|
||
{
|
||
int ptNum = (int)points.size();
|
||
SVzNL3DPoint centroid = { 0.0, 0.0, 0.0 };
|
||
if (ptNum == 0)
|
||
return centroid;
|
||
for (int i = 0; i < ptNum; i++)
|
||
{
|
||
centroid.x += points[i].x;
|
||
centroid.y += points[i].y;
|
||
}
|
||
centroid.x = centroid.x / ptNum;
|
||
centroid.y = centroid.y / ptNum;
|
||
return centroid;
|
||
}
|
||
|
||
SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, double matrix3d[9])
|
||
{
|
||
SVzNL3DPoint _r_pt;
|
||
_r_pt.x = pt3D.x * matrix3d[0] + pt3D.y * matrix3d[1] + pt3D.z * matrix3d[2];
|
||
_r_pt.y = pt3D.x * matrix3d[3] + pt3D.y * matrix3d[4] + pt3D.z * matrix3d[5];
|
||
_r_pt.z = pt3D.x * matrix3d[6] + pt3D.y * matrix3d[7] + pt3D.z * matrix3d[8];
|
||
return _r_pt;
|
||
}
|
||
|
||
//计算螺杆端部中心点位姿
|
||
void sx_hexHeadScrewMeasure(
|
||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||
bool isHorizonScan, //true:激光线平行槽道；false:激光线垂直槽道
|
||
const SSG_cornerParam cornerPara,
|
||
const SSG_outlierFilterParam filterParam,
|
||
const SSG_treeGrowParam growParam,
|
||
double rodDiameter,
|
||
std::vector<SSX_rodPoseInfo>& screwInfo,
|
||
int* errCode)
|
||
{
|
||
*errCode = 0;
|
||
int lineNum = (int)scanLines.size();
|
||
if (lineNum == 0)
|
||
{
|
||
*errCode = SG_ERR_3D_DATA_NULL;
|
||
return;
|
||
}
|
||
|
||
int linePtNum = (int)scanLines[0].size();
|
||
|
||
//判断数据格式是否为grid。算法只能处理grid数据格式
|
||
bool isGridData = true;
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
if (linePtNum != (int)scanLines[line].size())
|
||
{
|
||
isGridData = false;
|
||
break;
|
||
}
|
||
}
|
||
if (false == isGridData)//数据不是网格格式
|
||
{
|
||
*errCode = SG_ERR_NOT_GRID_FORMAT;
|
||
return;
|
||
}
|
||
|
||
std::vector< std::vector<SVzNL3DPosition>> data_lines;
|
||
if (false == isHorizonScan)
|
||
{
|
||
data_lines.resize(lineNum);
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
data_lines[line].insert(data_lines[line].end(), scanLines[line].begin(), scanLines[line].end());
|
||
for (int j = 0, j_max = (int)data_lines[line].size(); j < j_max; j++)
|
||
{
|
||
data_lines[line][j].nPointIdx = j;
|
||
scanLines[line][j].nPointIdx = 0; //转义复用
|
||
}
|
||
}
|
||
}
|
||
else
|
||
{
|
||
data_lines.resize(linePtNum);
|
||
for (int i = 0; i < linePtNum; i++)
|
||
data_lines[i].resize(lineNum);
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
for (int j = 0; j < linePtNum; j++)
|
||
{
|
||
scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
|
||
data_lines[j][line] = scanLines[line][j];
|
||
data_lines[j][line].pt3D.x = scanLines[line][j].pt3D.y;
|
||
data_lines[j][line].pt3D.y = scanLines[line][j].pt3D.x;
|
||
}
|
||
|
||
}
|
||
|
||
lineNum = linePtNum;
|
||
linePtNum = (int)data_lines[0].size();
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
for (int j = 0, j_max = (int)data_lines[line].size(); j < j_max; j++)
|
||
data_lines[line][j].nPointIdx = j;
|
||
}
|
||
}
|
||
|
||
std::vector<std::vector<SWD_segFeature>> arcFeatures;
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
if (line == 329)
|
||
int kkk = 1;
|
||
std::vector<SVzNL3DPosition>& lineData = data_lines[line];
|
||
|
||
//滤波，滤除异常点
|
||
sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam);
|
||
std::vector<SWD_segFeature> line_ringArcs;
|
||
int dataSize = (int)lineData.size();
|
||
SVzNLRangeD arcWidth;
|
||
arcWidth.min = rodDiameter / 2;
|
||
arcWidth.max = rodDiameter * 1.5;
|
||
//提取Arc特征
|
||
wd_getRingArcFeature(
|
||
lineData,
|
||
line, //当前扫描线序号
|
||
cornerPara,
|
||
arcWidth, //环宽度，以半径为基准，对应60度角
|
||
line_ringArcs //环
|
||
);
|
||
arcFeatures.push_back(line_ringArcs);
|
||
}
|
||
//特征生长
|
||
std::vector<SWD_segFeatureTree> growTrees;
|
||
wd_getSegFeatureGrowingTrees(
|
||
arcFeatures,
|
||
growTrees,
|
||
growParam);
|
||
|
||
if (growTrees.size() == 0)
|
||
{
|
||
*errCode = SG_ERR_NOT_GRID_FORMAT;
|
||
return;
|
||
}
|
||
int objNum = (int)growTrees.size();
|
||
|
||
//置标志，用于debug
|
||
for (int i = 0; i < objNum; i++)
|
||
{
|
||
int nodeNum = (int)growTrees[i].treeNodes.size();
|
||
for (int j = 0; j < nodeNum; j++)
|
||
{
|
||
int lineIdx, ptIdx;
|
||
if (false == isHorizonScan)
|
||
{
|
||
lineIdx = growTrees[i].treeNodes[j].lineIdx;
|
||
for (int m = growTrees[i].treeNodes[j].startPtIdx; m <= growTrees[i].treeNodes[j].endPtIdx; m++)
|
||
{
|
||
ptIdx = m;
|
||
scanLines[lineIdx][ptIdx].nPointIdx = 1;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
ptIdx = growTrees[i].treeNodes[j].lineIdx;
|
||
for (int m = growTrees[i].treeNodes[j].startPtIdx; m <= growTrees[i].treeNodes[j].endPtIdx; m++)
|
||
{
|
||
lineIdx = m;
|
||
scanLines[lineIdx][ptIdx].nPointIdx = 1;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
//逐个目标处理
|
||
for (int i = 0; i < objNum; i++)
|
||
{
|
||
//空间直线拟合
|
||
std::vector<SVzNL3DPoint> fitPoints;
|
||
std::vector<SVzNL2DPoint> fit2DPos;
|
||
int nodeSize = (int)growTrees[i].treeNodes.size();
|
||
for (int j = 0; j < nodeSize; j++)
|
||
{
|
||
SVzNL2DPoint arcPos;
|
||
SVzNL3DPoint a_pt = getArcPeak_parabolaFitting(data_lines, growTrees[i].treeNodes[j], arcPos);
|
||
//SVzNL3DPoint a_pt = getArcPeak(data_lines, growTrees[i].treeNodes[j], arcPos);
|
||
fitPoints.push_back(a_pt);
|
||
fit2DPos.push_back(arcPos);
|
||
}
|
||
if (fitPoints.size() < 27)
|
||
continue;
|
||
//去除头尾各5个点，防止在端部和根部扫描时的数据有干扰
|
||
fitPoints.erase(fitPoints.begin(), fitPoints.begin() + 10);
|
||
fit2DPos.erase(fit2DPos.begin(), fit2DPos.begin() + 10);
|
||
fitPoints.erase(fitPoints.end() - 5, fitPoints.end());
|
||
fit2DPos.erase(fit2DPos.end() - 5, fit2DPos.end());
|
||
//置标志
|
||
for (int j = 0; j < (int)fit2DPos.size(); j++)
|
||
{
|
||
int lineIdx, ptIdx;
|
||
if (false == isHorizonScan)
|
||
{
|
||
lineIdx = fit2DPos[j].x;
|
||
ptIdx = fit2DPos[j].y;
|
||
}
|
||
else
|
||
{
|
||
lineIdx = fit2DPos[j].y;
|
||
ptIdx = fit2DPos[j].x;
|
||
}
|
||
scanLines[lineIdx][ptIdx].nPointIdx = 2;
|
||
}
|
||
|
||
//拟合
|
||
SVzNL3DPoint P0_center, P1_dir;
|
||
bool result = fitLine3DLeastSquares(fitPoints, P0_center, P1_dir);
|
||
if (false == result)
|
||
continue;
|
||
//投影
|
||
//计算旋转向量
|
||
SVzNL3DPoint vector1 = P1_dir;
|
||
SVzNL3DPoint vector2 = { 0, 0, -1.0 };
|
||
SSG_planeCalibPara rotatePara = wd_computeRTMatrix( vector1, vector2);
|
||
//
|
||
SVzNL3DPoint P0_rotate = _ptRotate(P0_center, rotatePara.planeCalib);
|
||
SSG_ROIRectD roi_xoy;
|
||
roi_xoy.left = P0_rotate.x - rodDiameter * 2; //2D范围
|
||
roi_xoy.right = P0_rotate.x + rodDiameter * 2; //2D范围
|
||
roi_xoy.top = P0_rotate.y - rodDiameter * 2; //2D范围
|
||
roi_xoy.bottom = P0_rotate.y + rodDiameter * 2; //2D范围
|
||
|
||
#if 1
|
||
std::vector< SVzNL3DPoint> verifyData;
|
||
for (int m = 0; m < (int)fitPoints.size(); m++)
|
||
{
|
||
SVzNL3DPoint rPt = _ptRotate(fitPoints[m], rotatePara.planeCalib);
|
||
verifyData.push_back(rPt);
|
||
}
|
||
#endif
|
||
std::vector< SVzNL3DPoint> roiProjectionData;
|
||
xoyROIProjection(data_lines, rotatePara.planeCalib, roi_xoy, roiProjectionData);
|
||
//取端面
|
||
SVzNLRangeD zRange = getZRange(roiProjectionData);
|
||
SVzNLRangeD cutZRange;
|
||
cutZRange.min = zRange.min;
|
||
cutZRange.max = zRange.min + 5.0; //5mm的端面
|
||
std::vector<SVzNL3DPoint> surfacePoints;
|
||
zCutPointClouds(roiProjectionData, cutZRange, surfacePoints);
|
||
//计算中心点
|
||
SVzNL3DPoint projectionCenter;// = getXoYCentroid(surfacePoints);
|
||
SVzNL3DRangeD roi3D = wd_getPointCloudROI(surfacePoints);
|
||
projectionCenter.x = (roi3D.xRange.min + roi3D.xRange.max) / 2;
|
||
projectionCenter.y = (roi3D.yRange.min + roi3D.yRange.max) / 2;
|
||
projectionCenter.z = zRange.min;
|
||
//旋转回原坐标系
|
||
SVzNL3DPoint surfaceCenter = _ptRotate(projectionCenter, rotatePara.invRMatrix);
|
||
//生成Rod信息
|
||
SSX_rodPoseInfo a_rod;
|
||
a_rod.center = surfaceCenter;
|
||
a_rod.axialDir = P1_dir;
|
||
screwInfo.push_back(a_rod);
|
||
}
|
||
if (true == isHorizonScan)
|
||
{
|
||
int objNum = (int)screwInfo.size();
|
||
for (int i = 0; i < objNum; i++)
|
||
{
|
||
double tmp = screwInfo[i].center.x;
|
||
screwInfo[i].center.x = screwInfo[i].center.y;
|
||
screwInfo[i].center.y = tmp;
|
||
tmp = screwInfo[i].axialDir.x;
|
||
screwInfo[i].axialDir.x = screwInfo[i].axialDir.y;
|
||
screwInfo[i].axialDir.y = tmp;
|
||
//screwInfo[i].rotateAngle += 90;
|
||
}
|
||
}
|
||
return;
|
||
}
|
||
|
||
double _getListMeanZ(std::vector< SVzNL3DPosition>& listData)
|
||
{
|
||
if (listData.size() == 0)
|
||
return 0;
|
||
double meanZ = 0;
|
||
for (int i = 0; i < (int)listData.size(); i++)
|
||
meanZ += listData[i].pt3D.z;
|
||
meanZ = meanZ / (double)listData.size();
|
||
return meanZ;
|
||
}
|
||
|
||
SSG_ROIRectD _getListROI(std::vector< SVzNL3DPosition>& listData)
|
||
{
|
||
if (listData.size() == 0)
|
||
return { 0,0,0,0 };
|
||
SSG_ROIRectD roi = { listData[0].pt3D.x, listData[0].pt3D.x, listData[0].pt3D.y, listData[0].pt3D.y};
|
||
for (int i = 0; i < (int)listData.size(); i++)
|
||
{
|
||
roi.left = roi.left > listData[i].pt3D.x ? listData[i].pt3D.x : roi.left;
|
||
roi.right = roi.right < listData[i].pt3D.x ? listData[i].pt3D.x : roi.right;
|
||
roi.top = roi.top > listData[i].pt3D.y ? listData[i].pt3D.y : roi.top;
|
||
roi.bottom = roi.bottom < listData[i].pt3D.y ? listData[i].pt3D.y : roi.bottom;
|
||
}
|
||
return roi;
|
||
}
|
||
|
||
bool _compareByAngle(const SWD_polarPt& a, const SWD_polarPt& b) {
|
||
return a.angle < b.angle;
|
||
}
|
||
|
||
//计算封闭序列（首尾相连）在XOY平面内前向角和后向角以及拐角。 前向角：序号+； 后向角：序号-
|
||
void _computeClosedPntListDirCorners(std::vector<SWD_polarPt>& polarPoints, double scale, std::vector< SSG_dirCornerAngle>& dirCornerAngles)
|
||
{
|
||
int pntSize = (int)polarPoints.size();
|
||
//std::fill(dirCornerAngles.begin(), dirCornerAngles.end(), SSG_dirCornerAngle{ 0, 0, 0, 0, 0, 0 });
|
||
dirCornerAngles.resize(pntSize);
|
||
for (int i = 0; i < pntSize; i++)
|
||
{
|
||
if (i == 68)
|
||
int kkk = 1;
|
||
memset(&dirCornerAngles[i], 0, sizeof(SSG_dirCornerAngle));
|
||
SWD_polarPt& a_polarPt = polarPoints[i];
|
||
//前向寻找
|
||
int minus_i = -1;
|
||
for (int loop = i - 1; loop >= i - pntSize; loop--)
|
||
{
|
||
int j = loop >= 0 ? loop : (loop + pntSize);
|
||
double dist = sqrt(pow(polarPoints[i].x - polarPoints[j].x, 2) +
|
||
pow(polarPoints[i].y - polarPoints[j].y, 2));
|
||
if (dist >= scale)
|
||
{
|
||
minus_i = j;
|
||
break;
|
||
}
|
||
}
|
||
//后向寻找
|
||
int plus_i = -1;
|
||
for (int loop = i + 1; loop < i + pntSize; loop++)
|
||
{
|
||
int j = loop % pntSize;
|
||
double dist = sqrt(pow(polarPoints[i].x - polarPoints[j].x, 2) +
|
||
pow(polarPoints[i].y - polarPoints[j].y, 2));
|
||
if (dist >= scale)
|
||
{
|
||
plus_i = j;
|
||
break;
|
||
}
|
||
}
|
||
//计算拐角
|
||
if ((minus_i >= 0) && (plus_i >= 0))
|
||
{
|
||
double backwardAngle = atan2(polarPoints[i].y - polarPoints[minus_i].y, polarPoints[i].x - polarPoints[minus_i].x) * 180.0 / PI;
|
||
double forwardAngle = atan2(polarPoints[plus_i].y - polarPoints[i].y, polarPoints[plus_i].x - polarPoints[i].x) * 180.0 / PI;
|
||
dirCornerAngles[i].forwardAngle = forwardAngle;
|
||
dirCornerAngles[i].backwardAngle = backwardAngle;
|
||
|
||
double corner = forwardAngle - backwardAngle;
|
||
if (corner < -180)
|
||
corner += 360;
|
||
else if (corner > 180)
|
||
corner = corner - 360;
|
||
|
||
dirCornerAngles[i].flag = 0;
|
||
//过滤掉由于极角和方向角相近产生的轮廓点位序错误导致的corner错误
|
||
double polarAngle = dirCornerAngles[i].point.angle;
|
||
double angleDiff1 = abs(polarAngle - dirCornerAngles[i].forwardAngle);
|
||
if (angleDiff1 > 180)
|
||
angleDiff1 = 360 - angleDiff1;
|
||
double angleDiff2 = abs(polarAngle - dirCornerAngles[i].backwardAngle);
|
||
if (angleDiff2 > 180)
|
||
angleDiff2 = 360 - angleDiff2;
|
||
if ((angleDiff1 < 10) || (angleDiff2 < 10)) //过滤掉边与极线方向相近导致的波动
|
||
{
|
||
if (dirCornerAngles[i].corner > 160) //乱序产生角度错误
|
||
{
|
||
dirCornerAngles[i].flag = -1;
|
||
}
|
||
}
|
||
|
||
dirCornerAngles[i].corner = corner; //图像坐标系与正常坐标系y方向相反，所以有“-”号
|
||
dirCornerAngles[i].pntIdx = i;
|
||
dirCornerAngles[i].forward_pntIdx = plus_i;
|
||
dirCornerAngles[i].backward_pntIdx = minus_i;
|
||
dirCornerAngles[i].point = polarPoints[i];
|
||
}
|
||
}
|
||
}
|
||
|
||
//提取corner极值（较早实现函数可以使用此函数进行代码优化）
|
||
void _searchPlusCornerPeaks(
|
||
std::vector< SSG_dirCornerAngle>& corners,
|
||
double cutAngleTh,
|
||
std::vector< SSG_dirCornerAngle>& cornerPlusPeaks
|
||
)
|
||
{
|
||
std::vector<SSG_dirCornerAngle> peakCorners;
|
||
int cornerSize = (int)corners.size();
|
||
//搜索拐角极值
|
||
int _state = 0;
|
||
int pre_i = -1;
|
||
int sEdgePtIdx = -1;
|
||
int eEdgePtIdx = -1;
|
||
SSG_dirCornerAngle* pre_data = NULL;
|
||
for (int i = 0, i_max = cornerSize; i < i_max; i++)
|
||
{
|
||
if (i == 451)
|
||
int kkk = 1;
|
||
SSG_dirCornerAngle* curr_data = &corners[i];
|
||
if (curr_data->flag < 0)
|
||
continue;
|
||
|
||
if (NULL == pre_data)
|
||
{
|
||
sEdgePtIdx = i;
|
||
eEdgePtIdx = i;
|
||
pre_data = curr_data;
|
||
pre_i = i;
|
||
continue;
|
||
}
|
||
|
||
eEdgePtIdx = i;
|
||
double cornerDiff = curr_data->corner - pre_data->corner;
|
||
switch (_state)
|
||
{
|
||
case 0: //初态
|
||
if (cornerDiff < 0) //下降
|
||
{
|
||
_state = 2;
|
||
}
|
||
else if (cornerDiff > 0) //上升
|
||
{
|
||
_state = 1;
|
||
}
|
||
break;
|
||
case 1: //上升
|
||
if (cornerDiff < 0) //下降
|
||
{
|
||
if (pre_data->corner > cutAngleTh) //截角门限，滤除点波动导致的角度变化
|
||
peakCorners.push_back(*pre_data);
|
||
_state = 2;
|
||
}
|
||
break;
|
||
case 2: //下降
|
||
if (cornerDiff > 0) // 上升
|
||
_state = 1;
|
||
break;
|
||
default:
|
||
_state = 0;
|
||
break;
|
||
}
|
||
pre_data = curr_data;
|
||
pre_i = i;
|
||
}
|
||
//注意：最后一个不处理，为基座位置
|
||
|
||
//极小值点（峰顶）
|
||
//极值比较，在尺度窗口下寻找局部极值点
|
||
for (int i = 0, i_max = (int)peakCorners.size(); i < i_max; i++)
|
||
{
|
||
bool isPeak = true;
|
||
SSG_dirCornerAngle& a_dirAngle = peakCorners[i];
|
||
int curr_dist1 = a_dirAngle.pntIdx - a_dirAngle.backward_pntIdx;
|
||
if (curr_dist1 < 0)
|
||
curr_dist1 += cornerSize;
|
||
int curr_dist2 = a_dirAngle.forward_pntIdx - a_dirAngle.pntIdx;
|
||
if (curr_dist2 < 0)
|
||
curr_dist2 += cornerSize;
|
||
|
||
//minus方向寻找
|
||
int minus_i = i;
|
||
while (1)
|
||
{
|
||
minus_i--;
|
||
if (minus_i < 0)
|
||
minus_i += i_max;
|
||
SSG_dirCornerAngle& minus_dirAngle = peakCorners[minus_i];
|
||
int pntDist_0 = a_dirAngle.pntIdx - minus_dirAngle.pntIdx;
|
||
if (pntDist_0 < 0)
|
||
pntDist_0 += cornerSize;
|
||
if (pntDist_0 > curr_dist1)
|
||
break;
|
||
|
||
if (a_dirAngle.corner < minus_dirAngle.corner)
|
||
{
|
||
isPeak = false;
|
||
break;
|
||
}
|
||
}
|
||
//plus方向寻找
|
||
int plus_i = i;
|
||
while (1)
|
||
{
|
||
plus_i++;
|
||
if (plus_i >= i_max)
|
||
plus_i = 0;
|
||
SSG_dirCornerAngle& plus_dirAngle = peakCorners[plus_i];
|
||
int pntDist_0 = plus_dirAngle.pntIdx - a_dirAngle.pntIdx;
|
||
if (pntDist_0 < 0)
|
||
pntDist_0 += cornerSize;
|
||
if (pntDist_0 > curr_dist2)
|
||
break;
|
||
|
||
if (a_dirAngle.corner < plus_dirAngle.corner)
|
||
{
|
||
isPeak = false;
|
||
break;
|
||
}
|
||
}
|
||
if (true == isPeak)
|
||
{
|
||
double corner_curr = a_dirAngle.corner;
|
||
double corner_1 = corners[a_dirAngle.forward_pntIdx].corner;
|
||
double corner_2 = corners[a_dirAngle.backward_pntIdx].corner;
|
||
double diff_1 = corner_curr - corner_1;
|
||
double diff_2 = corner_curr - corner_2;
|
||
if ((diff_1 > corner_curr / 4) && (diff_2 > corner_curr / 4))
|
||
{
|
||
double polarAngle = a_dirAngle.point.angle;
|
||
double angleDiff1 = abs(polarAngle - a_dirAngle.forwardAngle);
|
||
if (angleDiff1 > 180)
|
||
angleDiff1 = 360 - angleDiff1;
|
||
double angleDiff2 = abs(polarAngle - a_dirAngle.backwardAngle);
|
||
if (angleDiff2 > 180)
|
||
angleDiff2 = 360 - angleDiff2;
|
||
if ((angleDiff1 < 10) || (angleDiff2 < 10)) //过滤掉边与极线方向相近导致的波动
|
||
{
|
||
if (a_dirAngle.corner < 160) //乱序产生角度错误
|
||
{
|
||
a_dirAngle.flag = 0;
|
||
cornerPlusPeaks.push_back(a_dirAngle);
|
||
}
|
||
}
|
||
else
|
||
{
|
||
a_dirAngle.flag = 0;
|
||
cornerPlusPeaks.push_back(a_dirAngle);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
void _updateRoi3D(SVzNL3DRangeD& roi, SVzNL3DPoint& a_pt)
|
||
{
|
||
if (a_pt.z > 1E-4)
|
||
{
|
||
if (roi.zRange.max < 0)
|
||
{
|
||
roi.xRange.min = a_pt.x;
|
||
roi.xRange.max = a_pt.x;
|
||
roi.yRange.min = a_pt.y;
|
||
roi.yRange.max = a_pt.y;
|
||
roi.zRange.min = a_pt.z;
|
||
roi.zRange.max = a_pt.z;
|
||
}
|
||
else
|
||
{
|
||
if (roi.xRange.min > a_pt.x)
|
||
roi.xRange.min = a_pt.x;
|
||
if (roi.xRange.max < a_pt.x)
|
||
roi.xRange.max = a_pt.x;
|
||
if (roi.yRange.min > a_pt.y)
|
||
roi.yRange.min = a_pt.y;
|
||
if (roi.yRange.max < a_pt.y)
|
||
roi.yRange.max = a_pt.y;
|
||
if (roi.zRange.min > a_pt.z)
|
||
roi.zRange.min = a_pt.z;
|
||
if (roi.zRange.max < a_pt.z)
|
||
roi.zRange.max = a_pt.z;
|
||
}
|
||
}
|
||
return;
|
||
}
|
||
|
||
//计算定位盘中心点位姿
|
||
SSX_platePoseInfo sx_getLocationPlatePose(
|
||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||
const SSG_cornerParam cornerPara,
|
||
int* errCode)
|
||
{
|
||
*errCode = 0;
|
||
SSX_platePoseInfo resultPose;
|
||
resultPose.center = { 0, 0, 0 };
|
||
resultPose.normalDir = { 0, 0, 0 };
|
||
resultPose.holeLT = { 0, 0, 0 };
|
||
resultPose.holeRB = { 0, 0, 0 };
|
||
resultPose.xDir = { 0, 0, 0 };
|
||
resultPose.yDir = { 0, 0, 0 };
|
||
int lineNum = (int)scanLines.size();
|
||
if (lineNum == 0)
|
||
{
|
||
*errCode = SG_ERR_3D_DATA_NULL;
|
||
return resultPose;
|
||
}
|
||
|
||
int linePtNum = (int)scanLines[0].size();
|
||
|
||
//判断数据格式是否为grid。算法只能处理grid数据格式
|
||
bool isGridData = true;
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
if (linePtNum != (int)scanLines[line].size())
|
||
{
|
||
isGridData = false;
|
||
break;
|
||
}
|
||
}
|
||
if (false == isGridData)//数据不是网格格式
|
||
{
|
||
*errCode = SG_ERR_NOT_GRID_FORMAT;
|
||
return resultPose;
|
||
}
|
||
|
||
//产生水平扫描数据
|
||
std::vector< std::vector<SVzNL3DPosition>> scanLines_h;
|
||
scanLines_h.resize(linePtNum);
|
||
for (int i = 0; i < linePtNum; i++)
|
||
scanLines_h[i].resize(lineNum);
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
for (int j = 0; j < linePtNum; j++)
|
||
{
|
||
scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
|
||
scanLines_h[j][line] = scanLines[line][j];
|
||
scanLines_h[j][line].pt3D.x = scanLines[line][j].pt3D.y;
|
||
scanLines_h[j][line].pt3D.y = scanLines[line][j].pt3D.x;
|
||
}
|
||
}
|
||
for (int line = 0; line < linePtNum; line++)
|
||
{
|
||
for (int j = 0, j_max = (int)scanLines_h[line].size(); j < j_max; j++)
|
||
scanLines_h[line][j].nPointIdx = j;
|
||
}
|
||
|
||
//获取定位盘外表面端点
|
||
//垂直方向
|
||
std::vector<std::vector<int>> flags;
|
||
flags.resize(lineNum);
|
||
for (int i = 0; i < lineNum; i++)
|
||
{
|
||
flags[i].resize(linePtNum);
|
||
std::fill(flags[i].begin(), flags[i].end(), 0);
|
||
}
|
||
|
||
std::vector< SVzNL3DPosition> endingPts;
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
std::vector< SVzNL3DPosition> vldPts;
|
||
std::vector<SSG_RUN_EX> segs;
|
||
std::vector<int> backIndexing;
|
||
backIndexing.resize(scanLines[line].size());
|
||
wd_lineDataSegment_zDist(
|
||
scanLines[line],
|
||
vldPts,
|
||
segs,
|
||
backIndexing,
|
||
cornerPara
|
||
);
|
||
for (int i = 0; i < (int)segs.size(); i++)
|
||
{
|
||
int idx_0 = segs[i].start;
|
||
int idx_1 = segs[i].start + segs[i].len - 1;
|
||
SVzNL3DPosition pt_0 = vldPts[idx_0];
|
||
SVzNL3DPosition pt_1 = vldPts[idx_1];
|
||
flags[line][pt_0.nPointIdx] = 1;
|
||
flags[line][pt_1.nPointIdx] = 1;
|
||
pt_0.nPointIdx = pt_0.nPointIdx & 0xffff | (line << 16);
|
||
pt_1.nPointIdx = pt_1.nPointIdx & 0xffff | (line << 16);
|
||
endingPts.push_back(pt_0);
|
||
endingPts.push_back(pt_1);
|
||
}
|
||
}
|
||
for (int line = 0; line < linePtNum; line++)
|
||
{
|
||
std::vector< SVzNL3DPosition> vldPts;
|
||
std::vector<SSG_RUN_EX> segs;
|
||
std::vector<int> backIndexing;
|
||
backIndexing.resize(scanLines_h[line].size());
|
||
wd_lineDataSegment_zDist(
|
||
scanLines_h[line],
|
||
vldPts,
|
||
segs,
|
||
backIndexing,
|
||
cornerPara
|
||
);
|
||
for (int i = 0; i < (int)segs.size(); i++)
|
||
{
|
||
int idx_0 = segs[i].start;
|
||
int idx_1 = segs[i].start + segs[i].len - 1;
|
||
SVzNL3DPosition pt_0 = vldPts[idx_0];
|
||
SVzNL3DPosition pt_1 = vldPts[idx_1];
|
||
if (0 == flags[pt_0.nPointIdx][line])//不和垂直特征提取的点重复
|
||
{
|
||
flags[pt_0.nPointIdx][line] = 1;
|
||
pt_0.pt3D = scanLines[pt_0.nPointIdx][line].pt3D; //取原始点
|
||
pt_0.nPointIdx = (pt_0.nPointIdx <<16) | (line & 0xffff);
|
||
endingPts.push_back(pt_0);
|
||
}
|
||
if (0 == flags[pt_1.nPointIdx][line]) //不和垂直特征提取的点重复
|
||
{
|
||
flags[pt_1.nPointIdx][line] = 1;
|
||
pt_1.pt3D = scanLines[pt_1.nPointIdx][line].pt3D; //取原始点
|
||
pt_1.nPointIdx = (pt_1.nPointIdx << 16) | (line & 0xffff);
|
||
endingPts.push_back(pt_1);
|
||
}
|
||
}
|
||
}
|
||
//标注
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
for (int j = 0; j < linePtNum; j++)
|
||
scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
|
||
}
|
||
for (int i = 0; i < (int)endingPts.size(); i++)
|
||
{
|
||
int line = endingPts[i].nPointIdx >> 16;
|
||
int ptIdx = endingPts[i].nPointIdx & 0x0000FFFF;
|
||
scanLines[line][ptIdx].nPointIdx = 1;
|
||
}
|
||
|
||
//聚类
|
||
//内部参数
|
||
int clusterCheckWin = 5;
|
||
double clusterDist = 5.0;
|
||
double topLayerThickness = 10.0;
|
||
double centerZ_R = 20.0;
|
||
int distType = 1; //0 - 2d distance; 1- 3d distance
|
||
std::vector<std::vector< SVzNL3DPosition>> objClusters; //result
|
||
wd_pointClustering_speedUp(
|
||
endingPts,
|
||
lineNum, linePtNum, clusterCheckWin, //搜索窗口
|
||
clusterDist,
|
||
distType,
|
||
objClusters //result
|
||
);
|
||
|
||
//判断上表面外轮廓
|
||
std::vector<double> objMeanZ;
|
||
objMeanZ.resize(objClusters.size());
|
||
std::vector< SSG_ROIRectD> objROIs;
|
||
objROIs.resize(objClusters.size());
|
||
//过滤出最小范围的Z
|
||
double minMeanZ = -1;
|
||
for (int i = 0; i < (int)objClusters.size(); i++)
|
||
{
|
||
SSG_ROIRectD a_roi = _getListROI(objClusters[i]);
|
||
objROIs[i] = a_roi;
|
||
double w = a_roi.right - a_roi.left;
|
||
double h = a_roi.bottom - a_roi.top;
|
||
|
||
double meanZ = _getListMeanZ(objClusters[i]);
|
||
objMeanZ[i] = meanZ;
|
||
if ( (meanZ > 1e-4) && (w > 150) && (h > 100))
|
||
{
|
||
if (minMeanZ < 0)
|
||
minMeanZ = meanZ;
|
||
else
|
||
minMeanZ = minMeanZ > meanZ ? meanZ : minMeanZ;
|
||
}
|
||
}
|
||
//选出ROI最大的目标作为顶部轮廓
|
||
int objIdx = -1;
|
||
double maxSize = 0;
|
||
for (int i = 0; i < (int)objClusters.size(); i++)
|
||
{
|
||
if (objMeanZ[i] > 1e-4)
|
||
{
|
||
double zDist = objMeanZ[i] - minMeanZ;
|
||
if (zDist < topLayerThickness) //同在顶层
|
||
{
|
||
double size = (objROIs[i].right - objROIs[i].left) * (objROIs[i].bottom - objROIs[i].top);
|
||
if (maxSize < -1e-4)
|
||
{
|
||
maxSize = size;
|
||
objIdx = i;
|
||
}
|
||
else
|
||
{
|
||
if (maxSize < size)
|
||
{
|
||
maxSize = size;
|
||
objIdx = i;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
if (objIdx < 0)
|
||
{
|
||
*errCode = SG_ERR_ZERO_OBJECTS;
|
||
return resultPose;
|
||
}
|
||
//标注
|
||
for (int i = 0; i < (int)objClusters[objIdx].size(); i++)
|
||
{
|
||
int line = objClusters[objIdx][i].nPointIdx >> 16;
|
||
int ptIdx = objClusters[objIdx][i].nPointIdx & 0x0000FFFF;
|
||
scanLines[line][ptIdx].nPointIdx = 2;
|
||
}
|
||
|
||
//拟合平面
|
||
std::vector<cv::Point3f> Points3ds;
|
||
std::vector<SWD_polarPt> polarPoints;
|
||
for (int i = 0; i < (int)objClusters[objIdx].size(); i++)
|
||
{
|
||
cv::Point3f a_pt = cv::Point3f(objClusters[objIdx][i].pt3D.x, objClusters[objIdx][i].pt3D.y, objClusters[objIdx][i].pt3D.z);
|
||
Points3ds.push_back(a_pt);
|
||
SWD_polarPt a_polarPt;
|
||
a_polarPt.lineIdx = objClusters[objIdx][i].nPointIdx >> 16;
|
||
a_polarPt.ptIdx = objClusters[objIdx][i].nPointIdx & 0x0000FFFF;
|
||
a_polarPt.R = 0;
|
||
a_polarPt.angle = 0;
|
||
a_polarPt.x = objClusters[objIdx][i].pt3D.x;
|
||
a_polarPt.y = objClusters[objIdx][i].pt3D.y;
|
||
a_polarPt.z = objClusters[objIdx][i].pt3D.z;
|
||
polarPoints.push_back(a_polarPt);
|
||
}
|
||
//计算面参数: z = Ax + By + C
|
||
//res: [0]=A, [1]= B, [2]=-1.0, [3]=C,
|
||
#if 1
|
||
std::vector<cv::Point3f> out_inliers;
|
||
Plane res = ransacFitPlane( Points3ds, out_inliers );
|
||
if (res.C < 0)
|
||
{
|
||
res.A = -res.A;
|
||
res.B = -res.B;
|
||
res.C = -res.C;
|
||
res.D = -res.D;
|
||
}
|
||
#else
|
||
Plane res = robustFitPlane(Points3ds);
|
||
#endif
|
||
//std::vector<double> res;
|
||
//vzCaculateLaserPlane(Points3ds, res);
|
||
//计算投影向量
|
||
SVzNL3DPoint vec_1 = {res.A, res.B, res.C};
|
||
SVzNL3DPoint vec_2 = { 0, 0, 1.0 };
|
||
SSG_planeCalibPara poseR = wd_computeRTMatrix(vec_1, vec_2);
|
||
|
||
//投影
|
||
double normDataPlane = sqrt(res.A * res.A + res.B * res.B + res.C * res.C);
|
||
std::vector<cv::Point3f> projectPoints3ds;
|
||
projectPoints3ds.resize(Points3ds.size());
|
||
double sum_x = 0, sum_y = 0, sum_z = 0;
|
||
double sumZConter = 0;
|
||
for (int i = 0; i < (int)Points3ds.size(); i++)
|
||
{
|
||
double distToPlane = abs(res.A * Points3ds[i].x + res.B * Points3ds[i].y + res.C * Points3ds[i].z + res.D) / normDataPlane;
|
||
double x = Points3ds[i].x * poseR.planeCalib[0] + Points3ds[i].y * poseR.planeCalib[1] + Points3ds[i].z * poseR.planeCalib[2];
|
||
double y = Points3ds[i].x * poseR.planeCalib[3] + Points3ds[i].y * poseR.planeCalib[4] + Points3ds[i].z * poseR.planeCalib[5];
|
||
double z = Points3ds[i].x * poseR.planeCalib[6] + Points3ds[i].y * poseR.planeCalib[7] + Points3ds[i].z * poseR.planeCalib[8];
|
||
projectPoints3ds[i] = cv::Point3f(x, y, z);
|
||
polarPoints[i].x = x;
|
||
polarPoints[i].y = y;
|
||
polarPoints[i].z = z;
|
||
sum_x += x;
|
||
sum_y += y;
|
||
|
||
if (distToPlane < 2.0)
|
||
{
|
||
sum_z += z;
|
||
sumZConter++;
|
||
}
|
||
|
||
}
|
||
|
||
for (int i = 0; i < lineNum; i++)
|
||
{
|
||
if (i == 14)
|
||
int kkk = 1;
|
||
//行处理
|
||
//调平，去除地面
|
||
lineDataRT_vector(scanLines[i], poseR.planeCalib, -1);
|
||
}
|
||
|
||
//计算质心
|
||
double center_x = sum_x / (double)Points3ds.size();
|
||
double center_y = sum_y / (double)Points3ds.size();
|
||
double center_z = sum_z / (double)sumZConter;
|
||
//计算极坐标的R和Theta
|
||
for (int pi = 0; pi < (int)polarPoints.size(); 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 < (int)polarPoints.size(); pi++)
|
||
polarPoints[pi].cptIndex = pi; // index
|
||
|
||
//计算有序边缘点的角度变化 2026.03.06
|
||
std::vector< SSG_dirCornerAngle> dirCornerAngles;
|
||
double dirAngleScale = 20.0;
|
||
_computeClosedPntListDirCorners(polarPoints, dirAngleScale, dirCornerAngles);
|
||
//提取方向角拐点(正）极值
|
||
std::vector< SSG_dirCornerAngle> cornerPlusPeaks;
|
||
double minCutAngleTh = 30;
|
||
_searchPlusCornerPeaks(dirCornerAngles, minCutAngleTh, cornerPlusPeaks);
|
||
|
||
#if 1
|
||
int cornerIdx_LT = -1;
|
||
int cornerIdx_RB = -1;
|
||
for (int i = 0; i < (int)cornerPlusPeaks.size(); i++)
|
||
{
|
||
if ((cornerPlusPeaks[i].point.angle > 0) && (cornerPlusPeaks[i].point.angle < 90))
|
||
cornerIdx_LT = i;
|
||
if ((cornerPlusPeaks[i].point.angle > 180) && (cornerPlusPeaks[i].point.angle < 270))
|
||
cornerIdx_RB = i;
|
||
}
|
||
if ((cornerIdx_LT < 0) || (cornerIdx_RB < 0))
|
||
{
|
||
*errCode = SX_ERR_UNKNOWN_PLATE_DIR;
|
||
return resultPose;
|
||
}
|
||
SSG_dirCornerAngle& corner_LT = cornerPlusPeaks[cornerIdx_LT];
|
||
SSG_dirCornerAngle& corner_RB = cornerPlusPeaks[cornerIdx_RB];
|
||
|
||
//Z切割
|
||
//内部参数
|
||
double zTh_min = center_z - 1.0;
|
||
double zTh_max = center_z + 5.0;
|
||
std::vector< std::vector<SVzNL3DPosition>> zCutLines;
|
||
zCutLines.resize(scanLines.size());
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
//行处理
|
||
zCutLines[line].resize(linePtNum);
|
||
for (int j = 0; j < linePtNum; j++)
|
||
{
|
||
zCutLines[line][j] = scanLines[line][j];
|
||
if ((scanLines[line][j].pt3D.z < zTh_min) || (scanLines[line][j].pt3D.z > zTh_max))
|
||
zCutLines[line][j].pt3D = { 0.0, 0.0, 0.0 };
|
||
}
|
||
}
|
||
//孔处理
|
||
std::vector<SSG_intPair> validObjects;
|
||
std::vector<SWD_segFeatureTree> holeSegTrees_v;
|
||
std::vector<SWD_segFeatureTree> holeSegTrees_h;
|
||
SSG_lineSegParam hole_lineSegPara;
|
||
hole_lineSegPara.distScale = 3.0;
|
||
hole_lineSegPara.segGapTh_y = 15.0; //
|
||
hole_lineSegPara.segGapTh_z = 0.0; //z方向间隔大于10mm认为是分段
|
||
SSG_outlierFilterParam hole_filterParam;
|
||
hole_filterParam.continuityTh = 20.0; //噪声滤除。当相邻点的z跳变大于此门限时，检查是否为噪声。若长度小于outlierLen， 视为噪声
|
||
hole_filterParam.outlierTh = 5;
|
||
SSG_treeGrowParam hole_growParam;
|
||
hole_growParam.maxLineSkipNum = 2;
|
||
hole_growParam.yDeviation_max = 4.0;
|
||
hole_growParam.maxSkipDistance = 2.0;
|
||
hole_growParam.zDeviation_max = 10.0;//
|
||
hole_growParam.minLTypeTreeLen = 2.0; //mm
|
||
hole_growParam.minVTypeTreeLen = 2.0; //mm
|
||
double valieCommonNumRatio = 0.25;
|
||
WD_getHoleInfo(
|
||
zCutLines,
|
||
hole_lineSegPara,
|
||
hole_filterParam,
|
||
hole_growParam,
|
||
valieCommonNumRatio,
|
||
holeSegTrees_v,
|
||
holeSegTrees_h,
|
||
validObjects
|
||
);
|
||
|
||
//寻找左上孔和右下孔
|
||
for (int i = 0; i < lineNum; i++)
|
||
{
|
||
for (int j = 0; j < (int)scanLines[i].size(); j++)
|
||
zCutLines[i][j].nPointIdx = 0; //清零
|
||
}
|
||
//生成聚类信息，
|
||
std::vector<std::vector< SVzNL2DPoint>> clusters; //只记录位置
|
||
std::vector<SVzNL3DRangeD> clustersRoi3D;
|
||
for (int i = 0; i < (int)validObjects.size(); i++)
|
||
{
|
||
std::vector< SVzNL2DPoint> a_cluster;
|
||
SVzNL3DRangeD a_roi3D = { {-1, -1}, {-1, -1}, {-1, -1 } };
|
||
|
||
int vTreeIdx = validObjects[i].data_0;
|
||
int hTreeIdx = validObjects[i].data_1;
|
||
for (int m = 0; m < (int)holeSegTrees_v[vTreeIdx].treeNodes.size(); m++)
|
||
{
|
||
SWD_segFeature& a_seg = holeSegTrees_v[vTreeIdx].treeNodes[m];
|
||
if (zCutLines[a_seg.lineIdx][a_seg.endPtIdx].nPointIdx == 0)
|
||
{
|
||
zCutLines[a_seg.lineIdx][a_seg.endPtIdx].nPointIdx = vTreeIdx + 1; // 0x01;
|
||
scanLines[a_seg.lineIdx][a_seg.endPtIdx].nPointIdx = vTreeIdx + 3; // 0x01;
|
||
SVzNL2DPoint a_pos = { a_seg.lineIdx , a_seg.endPtIdx };
|
||
a_cluster.push_back(a_pos);
|
||
_updateRoi3D(a_roi3D, zCutLines[a_seg.lineIdx][a_seg.endPtIdx].pt3D);
|
||
}
|
||
if (zCutLines[a_seg.lineIdx][a_seg.startPtIdx].nPointIdx == 0)
|
||
{
|
||
zCutLines[a_seg.lineIdx][a_seg.startPtIdx].nPointIdx = vTreeIdx + 1; // 0x01;
|
||
scanLines[a_seg.lineIdx][a_seg.startPtIdx].nPointIdx = vTreeIdx + 3; // 0x01;
|
||
SVzNL2DPoint a_pos = { a_seg.lineIdx , a_seg.startPtIdx };
|
||
a_cluster.push_back(a_pos);
|
||
_updateRoi3D(a_roi3D, zCutLines[a_seg.lineIdx][a_seg.startPtIdx].pt3D);
|
||
}
|
||
}
|
||
for (int m = 0; m < (int)holeSegTrees_h[hTreeIdx].treeNodes.size(); m++)
|
||
{
|
||
SWD_segFeature& a_seg = holeSegTrees_h[hTreeIdx].treeNodes[m];
|
||
if (zCutLines[a_seg.startPtIdx][a_seg.lineIdx].nPointIdx == 0)
|
||
{
|
||
zCutLines[a_seg.startPtIdx][a_seg.lineIdx].nPointIdx = hTreeIdx + 1; // 0x02;
|
||
scanLines[a_seg.startPtIdx][a_seg.lineIdx].nPointIdx = hTreeIdx + 4; // 0x02;
|
||
SVzNL2DPoint a_pos = { a_seg.startPtIdx , a_seg.lineIdx };
|
||
a_cluster.push_back(a_pos);
|
||
_updateRoi3D(a_roi3D, zCutLines[a_seg.startPtIdx][a_seg.lineIdx].pt3D);
|
||
}
|
||
if (zCutLines[a_seg.endPtIdx][a_seg.lineIdx].nPointIdx == 0)
|
||
{
|
||
zCutLines[a_seg.endPtIdx][a_seg.lineIdx].nPointIdx = hTreeIdx + 1; // 0x02;
|
||
scanLines[a_seg.endPtIdx][a_seg.lineIdx].nPointIdx = hTreeIdx + 4; // 0x02;
|
||
SVzNL2DPoint a_pos = { a_seg.endPtIdx , a_seg.lineIdx };
|
||
a_cluster.push_back(a_pos);
|
||
_updateRoi3D(a_roi3D, zCutLines[a_seg.endPtIdx][a_seg.lineIdx].pt3D);
|
||
}
|
||
}
|
||
clusters.push_back(a_cluster);
|
||
clustersRoi3D.push_back(a_roi3D);
|
||
}
|
||
|
||
//聚类结果分析:搜索距左上和右下点最后的目标
|
||
//内部参数
|
||
double minHoleSize = 10.0;
|
||
double maxHoleSize = 24.0;
|
||
double maxCornerHoleDistance = 40; //角点到孔的最大距离
|
||
int clusterSize = (int)clusters.size();
|
||
int clusterIdx_LT = -1;
|
||
double minDistLT = -1;
|
||
int clusterIdx_RB = -1;
|
||
double minDistRB = -1;
|
||
for (int i = 0; i < clusterSize; i++)
|
||
{
|
||
SVzNL3DRangeD& a_roi = clustersRoi3D[i];
|
||
double L = a_roi.xRange.max - a_roi.xRange.min;
|
||
double W = a_roi.yRange.max - a_roi.yRange.min;
|
||
double cx = (a_roi.xRange.max + a_roi.xRange.min) / 2;
|
||
double cy = (a_roi.yRange.max + a_roi.yRange.min) / 2;
|
||
|
||
if ((L > minHoleSize) && (L < maxHoleSize) && (W > minHoleSize) && (W < maxHoleSize))
|
||
{
|
||
double dist_1 = sqrt(pow(corner_LT.point.x - cx, 2) + pow(corner_LT.point.y - cy, 2));
|
||
if ((minDistLT < 0) || (minDistLT > dist_1))
|
||
{
|
||
minDistLT = dist_1;
|
||
clusterIdx_LT = i;
|
||
}
|
||
|
||
double dist_2 = sqrt(pow(corner_RB.point.x - cx, 2) + pow(corner_RB.point.y - cy, 2));
|
||
if ((minDistRB < 0) || (minDistRB > dist_2))
|
||
{
|
||
minDistRB = dist_2;
|
||
clusterIdx_RB = i;
|
||
}
|
||
}
|
||
}
|
||
if( (clusterIdx_LT < 0) || (clusterIdx_RB < 0) || (minDistLT > maxCornerHoleDistance) || (minDistRB > maxCornerHoleDistance))
|
||
{
|
||
*errCode = SX_ERR_UNKNOWN_PLATE_DIR;
|
||
return resultPose;
|
||
}
|
||
|
||
//目标圆拟合
|
||
std::vector<SVzNL3DPoint> pointArrayLT;
|
||
int clusterPtSizeLT = (int)clusters[clusterIdx_LT].size();
|
||
for (int i = 0; i < clusterPtSizeLT; i++)
|
||
{
|
||
SVzNL2DPoint a_pos = clusters[clusterIdx_LT][i];
|
||
int lineIdx = a_pos.x;
|
||
int ptIdx = a_pos.y;
|
||
SVzNL3DPoint a_pt3d = scanLines[lineIdx][ptIdx].pt3D;
|
||
pointArrayLT.push_back(a_pt3d);
|
||
}
|
||
//圆拟合
|
||
SVzNL3DPoint centerLT;
|
||
double radiusLT;
|
||
double err = fitCircleByLeastSquare(pointArrayLT, centerLT, radiusLT);
|
||
centerLT.z = center_z;
|
||
|
||
std::vector<SVzNL3DPoint> pointArrayRB;
|
||
int clusterPtSizeRB = (int)clusters[clusterIdx_RB].size();
|
||
for (int i = 0; i < clusterPtSizeRB; i++)
|
||
{
|
||
SVzNL2DPoint a_pos = clusters[clusterIdx_RB][i];
|
||
int lineIdx = a_pos.x;
|
||
int ptIdx = a_pos.y;
|
||
SVzNL3DPoint a_pt3d = scanLines[lineIdx][ptIdx].pt3D;
|
||
pointArrayRB.push_back(a_pt3d);
|
||
}
|
||
//圆拟合
|
||
SVzNL3DPoint centerRB;
|
||
double radiusRB;
|
||
err = fitCircleByLeastSquare(pointArrayRB, centerRB, radiusRB);
|
||
centerRB.z = center_z;
|
||
|
||
SVzNL3DPoint refVec = { (centerRB.x-centerLT.x)/2, (centerRB.y - centerLT.y)/2, center_z};
|
||
double angleToHorizon = -46; //两孔连线与水平线夹角46度
|
||
double angleToCenter = 3; //两孔连线中点与定位盘中心夹角3度
|
||
//逆时针旋转时 θ > 0 ；顺时针旋转时 θ < 0
|
||
SVzNL3DPoint rVec_1 = wd_rotate2D(refVec, angleToCenter);
|
||
center_x = rVec_1.x + centerLT.x;
|
||
center_y = rVec_1.y + centerLT.y;
|
||
//center_z = center_z + 34.0;
|
||
SVzNL3DPoint xDir = wd_rotate2D(refVec, angleToHorizon);
|
||
double normData = sqrt(pow(xDir.x, 2) + pow(xDir.y, 2));
|
||
xDir.x = xDir.x / normData;
|
||
xDir.y = xDir.y / normData;
|
||
xDir.z = 0;
|
||
|
||
resultPose.holeLT = centerLT;
|
||
resultPose.holeRB = centerRB;
|
||
|
||
center_z += 33.8; //定位盘中心点深33.8
|
||
#else
|
||
//生成直线段数据
|
||
int cornerIdx_0 = -1;
|
||
int cornerIdx_1 = -1;
|
||
for (int i = 0; i < (int)cornerPlusPeaks.size(); i++)
|
||
{
|
||
if ((cornerPlusPeaks[i].point.angle > 0) && (cornerPlusPeaks[i].point.angle < 90))
|
||
cornerIdx_0 = i;
|
||
if ((cornerPlusPeaks[i].point.angle > 90) && (cornerPlusPeaks[i].point.angle < 180))
|
||
cornerIdx_1 = i;
|
||
}
|
||
if ((cornerIdx_0 < 0) && (cornerIdx_1 < 0))
|
||
{
|
||
if (objIdx < 0)
|
||
{
|
||
*errCode = SX_ERR_UNKNOWN_PLATE_DIR;
|
||
return resultPose;
|
||
}
|
||
}
|
||
std::vector<SVzNL3DPoint> lineFittingPts;
|
||
double endingRemoveDist = 10.0;
|
||
double validFittingLen = 100;
|
||
if (cornerIdx_0 >= 0)
|
||
{
|
||
int startIdx = cornerPlusPeaks[cornerIdx_0].pntIdx;
|
||
double sx = polarPoints[startIdx].x;
|
||
double sy = polarPoints[startIdx].y;
|
||
for (int m = startIdx; m < (int)polarPoints.size(); m++)
|
||
{
|
||
double dist = sqrt(pow(polarPoints[m].x - sx, 2) + pow(polarPoints[m].y - sy, 2));
|
||
if ((dist >= endingRemoveDist) && (dist <= (validFittingLen + endingRemoveDist)))
|
||
{
|
||
SVzNL3DPoint a_fitPt = { polarPoints[m].x, polarPoints[m].y, polarPoints[m].z };
|
||
lineFittingPts.push_back(a_fitPt);
|
||
}
|
||
if (dist > (validFittingLen + endingRemoveDist))
|
||
break;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
int startIdx = cornerPlusPeaks[cornerIdx_1].pntIdx;
|
||
double sx = polarPoints[startIdx].x;
|
||
double sy = polarPoints[startIdx].y;
|
||
for (int m = startIdx; m >= 0; m--)
|
||
{
|
||
double dist = sqrt(pow(polarPoints[m].x - sx, 2) + pow(polarPoints[m].y - sy, 2));
|
||
if ((dist >= endingRemoveDist) && (dist <= (validFittingLen + endingRemoveDist)))
|
||
{
|
||
SVzNL3DPoint a_fitPt = { polarPoints[m].x, polarPoints[m].y, polarPoints[m].z };
|
||
lineFittingPts.push_back(a_fitPt);
|
||
}
|
||
if (dist > (validFittingLen + endingRemoveDist))
|
||
break;
|
||
}
|
||
}
|
||
//直线拟合
|
||
double _k, _b;
|
||
lineFitting(lineFittingPts, &_k, &_b);
|
||
double normData = sqrt(pow(_k, 2) + 1);
|
||
SVzNL3DPoint xDir = { 1.0/normData, _k/normData, 0.0 };
|
||
|
||
//计算4个点
|
||
SVzNL2DPointD top_pt = { center_x, center_y - centerZ_R };
|
||
SVzNL2DPointD bottom_pt = { center_x, center_y + centerZ_R };
|
||
SVzNL2DPointD left_pt = { center_x - centerZ_R, center_y};
|
||
SVzNL2DPointD roght_pt = { center_x + centerZ_R, center_y};
|
||
//计算Z
|
||
SVzNL3DPoint ptTop = { 0, 0, 0 };
|
||
SVzNL3DPoint ptBtm = { 0, 0, 0 };
|
||
SVzNL3DPoint ptLeft = { 0, 0, 0 };
|
||
SVzNL3DPoint ptRight = { 0, 0, 0 };
|
||
double minDistTop = -1, minDistBtm = -1, minDistLeft = -1, minDistRight = -1;
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
for (int j = 0; j < linePtNum; j++)
|
||
{
|
||
if (scanLines[line][j].pt3D.z > 1e-4)
|
||
{
|
||
//top
|
||
if (minDistTop < 1e-4)
|
||
{
|
||
ptTop = scanLines[line][j].pt3D;
|
||
minDistTop = sqrt(pow(top_pt.x - scanLines[line][j].pt3D.x, 2) + pow(top_pt.y - scanLines[line][j].pt3D.y, 2));
|
||
}
|
||
else
|
||
{
|
||
double dist = sqrt(pow(top_pt.x - scanLines[line][j].pt3D.x, 2) + pow(top_pt.y - scanLines[line][j].pt3D.y, 2));
|
||
if (minDistTop > dist)
|
||
{
|
||
ptTop = scanLines[line][j].pt3D;
|
||
minDistTop = dist;
|
||
}
|
||
}
|
||
//bottom
|
||
if (minDistBtm < 1e-4)
|
||
{
|
||
ptBtm = scanLines[line][j].pt3D;
|
||
minDistBtm = sqrt(pow(bottom_pt.x - scanLines[line][j].pt3D.x, 2) + pow(bottom_pt.y - scanLines[line][j].pt3D.y, 2));
|
||
}
|
||
else
|
||
{
|
||
double dist = sqrt(pow(bottom_pt.x - scanLines[line][j].pt3D.x, 2) + pow(bottom_pt.y - scanLines[line][j].pt3D.y, 2));
|
||
if (minDistBtm > dist)
|
||
{
|
||
ptBtm = scanLines[line][j].pt3D;
|
||
minDistBtm = dist;
|
||
}
|
||
}
|
||
//left
|
||
if (minDistLeft < 1e-4)
|
||
{
|
||
ptLeft = scanLines[line][j].pt3D;
|
||
minDistLeft = sqrt(pow(left_pt.x - scanLines[line][j].pt3D.x, 2) + pow(left_pt.y - scanLines[line][j].pt3D.y, 2));
|
||
}
|
||
else
|
||
{
|
||
double dist = sqrt(pow(left_pt.x - scanLines[line][j].pt3D.x, 2) + pow(left_pt.y - scanLines[line][j].pt3D.y, 2));
|
||
if (minDistLeft > dist)
|
||
{
|
||
ptLeft = scanLines[line][j].pt3D;
|
||
minDistLeft = dist;
|
||
}
|
||
}
|
||
//right
|
||
if (minDistRight < 1e-4)
|
||
{
|
||
ptRight = scanLines[line][j].pt3D;
|
||
minDistRight = sqrt(pow(roght_pt.x - scanLines[line][j].pt3D.x, 2) + pow(roght_pt.y - scanLines[line][j].pt3D.y, 2));
|
||
}
|
||
else
|
||
{
|
||
double dist = sqrt(pow(roght_pt.x - scanLines[line][j].pt3D.x, 2) + pow(roght_pt.y - scanLines[line][j].pt3D.y, 2));
|
||
if (minDistRight > dist)
|
||
{
|
||
ptRight = scanLines[line][j].pt3D;
|
||
minDistRight = dist;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
double center_z = (ptTop.z + ptBtm.z + ptLeft.z + ptRight.z) / 4;
|
||
#endif
|
||
resultPose.center = { center_x, center_y, center_z };
|
||
resultPose.xDir = { 0.0, 0.0, -1.0 }; // { 1.0, 0.0, 0 };
|
||
//resultPose.yDir = { 0.0, 1.0, 0 };
|
||
resultPose.normalDir = xDir; // { 1.0, 0.0, 0 };// { 0.0, 0.0, 1.0 };
|
||
//叉乘出y;
|
||
//向量叉乘
|
||
resultPose.yDir = vec3_cross(resultPose.normalDir, resultPose.xDir);
|
||
|
||
//旋转回去
|
||
for (int i = 0; i < lineNum; i++)
|
||
{
|
||
//行处理
|
||
//调平，去除地面
|
||
lineDataRT_vector(scanLines[i], poseR.invRMatrix, -1);
|
||
}
|
||
double x = resultPose.center.x * poseR.invRMatrix[0] + resultPose.center.y * poseR.invRMatrix[1] + resultPose.center.z * poseR.invRMatrix[2];
|
||
double y = resultPose.center.x * poseR.invRMatrix[3] + resultPose.center.y * poseR.invRMatrix[4] + resultPose.center.z * poseR.invRMatrix[5];
|
||
double z = resultPose.center.x * poseR.invRMatrix[6] + resultPose.center.y * poseR.invRMatrix[7] + resultPose.center.z * poseR.invRMatrix[8];
|
||
resultPose.center = { x, y, z };
|
||
x = resultPose.normalDir.x * poseR.invRMatrix[0] + resultPose.normalDir.y * poseR.invRMatrix[1] + resultPose.normalDir.z * poseR.invRMatrix[2];
|
||
y = resultPose.normalDir.x * poseR.invRMatrix[3] + resultPose.normalDir.y * poseR.invRMatrix[4] + resultPose.normalDir.z * poseR.invRMatrix[5];
|
||
z = resultPose.normalDir.x * poseR.invRMatrix[6] + resultPose.normalDir.y * poseR.invRMatrix[7] + resultPose.normalDir.z * poseR.invRMatrix[8];
|
||
resultPose.normalDir = { x, y, z };
|
||
|
||
x = resultPose.xDir.x * poseR.invRMatrix[0] + resultPose.xDir.y * poseR.invRMatrix[1] + resultPose.xDir.z * poseR.invRMatrix[2];
|
||
y = resultPose.xDir.x * poseR.invRMatrix[3] + resultPose.xDir.y * poseR.invRMatrix[4] + resultPose.xDir.z * poseR.invRMatrix[5];
|
||
z = resultPose.xDir.x * poseR.invRMatrix[6] + resultPose.xDir.y * poseR.invRMatrix[7] + resultPose.xDir.z * poseR.invRMatrix[8];
|
||
resultPose.xDir = { x, y, z };
|
||
|
||
x = resultPose.yDir.x * poseR.invRMatrix[0] + resultPose.yDir.y * poseR.invRMatrix[1] + resultPose.yDir.z * poseR.invRMatrix[2];
|
||
y = resultPose.yDir.x * poseR.invRMatrix[3] + resultPose.yDir.y * poseR.invRMatrix[4] + resultPose.yDir.z * poseR.invRMatrix[5];
|
||
z = resultPose.yDir.x * poseR.invRMatrix[6] + resultPose.yDir.y * poseR.invRMatrix[7] + resultPose.yDir.z * poseR.invRMatrix[8];
|
||
resultPose.yDir = { x, y, z };
|
||
|
||
x = resultPose.holeLT.x * poseR.invRMatrix[0] + resultPose.holeLT.y * poseR.invRMatrix[1] + resultPose.holeLT.z * poseR.invRMatrix[2];
|
||
y = resultPose.holeLT.x * poseR.invRMatrix[3] + resultPose.holeLT.y * poseR.invRMatrix[4] + resultPose.holeLT.z * poseR.invRMatrix[5];
|
||
z = resultPose.holeLT.x * poseR.invRMatrix[6] + resultPose.holeLT.y * poseR.invRMatrix[7] + resultPose.holeLT.z * poseR.invRMatrix[8];
|
||
resultPose.holeLT = { x, y, z };
|
||
|
||
x = resultPose.holeRB.x * poseR.invRMatrix[0] + resultPose.holeRB.y * poseR.invRMatrix[1] + resultPose.holeRB.z * poseR.invRMatrix[2];
|
||
y = resultPose.holeRB.x * poseR.invRMatrix[3] + resultPose.holeRB.y * poseR.invRMatrix[4] + resultPose.holeRB.z * poseR.invRMatrix[5];
|
||
z = resultPose.holeRB.x * poseR.invRMatrix[6] + resultPose.holeRB.y * poseR.invRMatrix[7] + resultPose.holeRB.z * poseR.invRMatrix[8];
|
||
resultPose.holeRB = { x, y, z };
|
||
|
||
return resultPose;
|
||
}
|
||
|
||
void rodAarcFeatueDetection(
|
||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||
const SSG_cornerParam cornerPara,
|
||
const SSG_outlierFilterParam filterParam,
|
||
const double rodDiameter,
|
||
std::vector<std::vector<SWD_rodArcFeature>>& arcFeatures)
|
||
{
|
||
int lineNum = (int)scanLines.size();
|
||
int linePtNum = (int)scanLines[0].size();
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
if (line == 1100)
|
||
int kkk = 1;
|
||
std::vector<SVzNL3DPosition>& lineData = scanLines[line];
|
||
|
||
//滤波，滤除异常点
|
||
sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam);
|
||
//提取rodArc特征
|
||
std::vector<SWD_rodArcFeature> line_rodArcs;
|
||
wd_getRodArcFeature_peakCornerMethod(lineData, line, rodDiameter/2, cornerPara, line_rodArcs );
|
||
arcFeatures.push_back(line_rodArcs);
|
||
}
|
||
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,
|
||
const SSG_cornerParam cornerPara,
|
||
const SSG_outlierFilterParam filterParam,
|
||
const SSG_treeGrowParam growParam,
|
||
const SSX_rodParam rodParam,
|
||
std::vector<SSX_rodPositionInfo>& rodInfo,
|
||
int* errCode)
|
||
{
|
||
*errCode = 0;
|
||
int lineNum = (int)scanLines.size();
|
||
if (lineNum == 0)
|
||
{
|
||
*errCode = SG_ERR_3D_DATA_NULL;
|
||
return;
|
||
}
|
||
|
||
int linePtNum = (int)scanLines[0].size();
|
||
|
||
//判断数据格式是否为grid。算法只能处理grid数据格式
|
||
bool isGridData = true;
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
if (linePtNum != (int)scanLines[line].size())
|
||
{
|
||
isGridData = false;
|
||
break;
|
||
}
|
||
}
|
||
if (false == isGridData)//数据不是网格格式
|
||
{
|
||
*errCode = SG_ERR_NOT_GRID_FORMAT;
|
||
return;
|
||
}
|
||
|
||
for (int i = 0, i_max = (int)scanLines.size(); i < i_max; i++)
|
||
{
|
||
if (i == 14)
|
||
int kkk = 1;
|
||
//行处理
|
||
//调平，去除地面
|
||
double cuttingZ = -1;
|
||
sx_rodPosition_lineDataR(scanLines[i], poseCalibPara.planeCalib, cuttingZ);
|
||
}
|
||
|
||
//生成水平扫描数据
|
||
std::vector<std::vector<SVzNL3DPosition>> hLines_raw;
|
||
hLines_raw.resize(linePtNum);
|
||
for (int i = 0; i < linePtNum; i++)
|
||
hLines_raw[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;
|
||
}
|
||
}
|
||
|
||
//在垂直方向上分别提取ARC特征，并进行特征生长
|
||
std::vector<std::vector<SWD_rodArcFeature>> arcFeatures_v;
|
||
rodAarcFeatueDetection( scanLines, cornerPara, filterParam, rodParam.diameter, arcFeatures_v);
|
||
//特征生长
|
||
std::vector<SWD_rodArcFeatureTree> rodArcTrees_v;
|
||
wd_getRodArcFeatureGrowingTrees(arcFeatures_v, rodArcTrees_v, growParam);
|
||
|
||
//水平方向
|
||
std::vector<std::vector<SWD_rodArcFeature>> arcFeatures_h;
|
||
rodAarcFeatueDetection(hLines_raw, cornerPara, filterParam, rodParam.diameter, arcFeatures_h);
|
||
//特征生长
|
||
std::vector<SWD_rodArcFeatureTree> rodArcTrees_h;
|
||
wd_getRodArcFeatureGrowingTrees(arcFeatures_h, rodArcTrees_h, growParam);
|
||
|
||
if ((rodArcTrees_v.size() == 0) && (rodArcTrees_h.size() == 0))
|
||
{
|
||
*errCode = SG_ERR_NOT_GRID_FORMAT;
|
||
return;
|
||
}
|
||
|
||
|
||
int objNum_v = (int)rodArcTrees_v.size();
|
||
int objNum_h = (int)rodArcTrees_h.size();
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
for (int j = 0; j < linePtNum; j++)
|
||
{
|
||
scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
|
||
}
|
||
}
|
||
//置标志，用于debug
|
||
for (int i = 0; i < objNum_v; i++)
|
||
{
|
||
int nodeNum = (int)rodArcTrees_v[i].treeNodes.size();
|
||
for (int j = 0; j < nodeNum; j++)
|
||
{
|
||
int lineIdx = rodArcTrees_v[i].treeNodes[j].lineIdx;
|
||
int centerPtIdx = rodArcTrees_v[i].treeNodes[j].peakPtIdx;
|
||
for (int m = rodArcTrees_v[i].treeNodes[j].startPtIdx; m <= rodArcTrees_v[i].treeNodes[j].endPtIdx; m++)
|
||
scanLines[lineIdx][m].nPointIdx = 1;
|
||
scanLines[lineIdx][centerPtIdx].nPointIdx |= 0x10;
|
||
}
|
||
}
|
||
//置标志，用于debug
|
||
for (int i = 0; i < objNum_h; i++)
|
||
{
|
||
int nodeNum = (int)rodArcTrees_h[i].treeNodes.size();
|
||
for (int j = 0; j < nodeNum; j++)
|
||
{
|
||
int ptIdx = rodArcTrees_h[i].treeNodes[j].lineIdx;
|
||
int centerLineIdx = rodArcTrees_h[i].treeNodes[j].peakPtIdx;
|
||
for (int m = rodArcTrees_h[i].treeNodes[j].startPtIdx; m <= rodArcTrees_h[i].treeNodes[j].endPtIdx; m++)
|
||
scanLines[m][ptIdx].nPointIdx |= 2;
|
||
scanLines[centerLineIdx][ptIdx].nPointIdx |= 0x20;
|
||
}
|
||
}
|
||
//目标判断
|
||
//(1)长度过滤
|
||
//垂直目标
|
||
for (int i = 0; i < (int)rodArcTrees_v.size(); i++)
|
||
{
|
||
int nodeSize = rodArcTrees_v[i].treeNodes.size();
|
||
SVzNL3DPoint startCenter = rodArcTrees_v[i].treeNodes[0].peakPt;
|
||
SVzNL3DPoint endCenter = rodArcTrees_v[i].treeNodes[nodeSize-1].peakPt;
|
||
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 dist1 = sqrt(pow(rodArcTrees_v[i].treeNodes[j].peakPt.x - startCenter.x, 2) +
|
||
pow(rodArcTrees_v[i].treeNodes[j].peakPt.y - startCenter.y, 2) +
|
||
pow(rodArcTrees_v[i].treeNodes[j].peakPt.z - startCenter.z, 2));
|
||
double dist2 = sqrt(pow(rodArcTrees_v[i].treeNodes[j].peakPt.x - endCenter.x, 2) +
|
||
pow(rodArcTrees_v[i].treeNodes[j].peakPt.y - endCenter.y, 2) +
|
||
pow(rodArcTrees_v[i].treeNodes[j].peakPt.z - endCenter.z, 2));
|
||
if ((dist1 > rodParam.diameter / 2) && (dist2 > rodParam.diameter / 2))
|
||
fittingPoints.push_back(rodArcTrees_v[i].treeNodes[j].peakPt);
|
||
}
|
||
if (fittingPoints.size() < 3)
|
||
continue;
|
||
SSX_rodPositionInfo a_objRod;
|
||
_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++)
|
||
{
|
||
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++)
|
||
sx_rodPosition_lineDataR(scanLines[i], poseCalibPara.invRMatrix, -1);
|
||
//将检测结果重新投射回原来的坐标系
|
||
for (int i = 0; i < (int)rodInfo.size(); i++)
|
||
{
|
||
SSX_rodPositionInfo& a_rod = rodInfo[i];
|
||
SVzNL3DPoint rawObj = _translatePoint(a_rod.center, poseCalibPara.invRMatrix);
|
||
a_rod.center = rawObj;
|
||
rawObj = _translatePoint(a_rod.axialDir, poseCalibPara.invRMatrix);
|
||
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;
|
||
}
|
||
|
||
//筑裕钢结构钢筋焊缝定位
|
||
void sx_rebarWeldSeamPositioning(
|
||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||
const SSG_planeCalibPara poseCalibPara,
|
||
const SSG_cornerParam cornerPara,
|
||
const SSG_outlierFilterParam filterParam,
|
||
const SSG_treeGrowParam growParam,
|
||
const SSX_rodParam rodParam,
|
||
std::vector<SSX_weldSeamInfo>& weldSeamInfo,
|
||
int* errCode)
|
||
{
|
||
*errCode = 0;
|
||
int lineNum = (int)scanLines.size();
|
||
if (lineNum == 0)
|
||
{
|
||
*errCode = SG_ERR_3D_DATA_NULL;
|
||
return;
|
||
}
|
||
|
||
int linePtNum = (int)scanLines[0].size();
|
||
|
||
//判断数据格式是否为grid。算法只能处理grid数据格式
|
||
bool isGridData = true;
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
if (linePtNum != (int)scanLines[line].size())
|
||
{
|
||
isGridData = false;
|
||
break;
|
||
}
|
||
}
|
||
if (false == isGridData)//数据不是网格格式
|
||
{
|
||
*errCode = SG_ERR_NOT_GRID_FORMAT;
|
||
return;
|
||
}
|
||
|
||
for (int i = 0, i_max = (int)scanLines.size(); i < i_max; i++)
|
||
{
|
||
if (i == 14)
|
||
int kkk = 1;
|
||
//行处理
|
||
//调平，去除地面
|
||
double cuttingZ = -1;
|
||
sx_rodPosition_lineDataR(scanLines[i], poseCalibPara.planeCalib, cuttingZ);
|
||
}
|
||
|
||
//寻找水平和垂直方向的钢筋
|
||
//生成水平扫描数据
|
||
std::vector<std::vector<SVzNL3DPosition>> hLines_raw;
|
||
hLines_raw.resize(linePtNum);
|
||
for (int i = 0; i < linePtNum; i++)
|
||
hLines_raw[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;
|
||
}
|
||
}
|
||
|
||
//在垂直方向上分别提取ARC特征，并进行特征生长
|
||
std::vector<std::vector<SWD_rodArcFeature>> arcFeatures_v;
|
||
rodAarcFeatueDetection(scanLines, cornerPara, filterParam, rodParam.diameter, arcFeatures_v);
|
||
//特征生长
|
||
std::vector<SWD_rodArcFeatureTree> rodArcTrees_v;
|
||
wd_getRodArcFeatureGrowingTrees(arcFeatures_v, rodArcTrees_v, growParam);
|
||
|
||
//水平方向
|
||
std::vector<std::vector<SWD_rodArcFeature>> arcFeatures_h;
|
||
rodAarcFeatueDetection(hLines_raw, cornerPara, filterParam, rodParam.diameter, arcFeatures_h);
|
||
//特征生长
|
||
std::vector<SWD_rodArcFeatureTree> rodArcTrees_h;
|
||
wd_getRodArcFeatureGrowingTrees(arcFeatures_h, rodArcTrees_h, growParam);
|
||
|
||
if ((rodArcTrees_v.size() == 0) && (rodArcTrees_h.size() == 0))
|
||
{
|
||
*errCode = SG_ERR_NOT_GRID_FORMAT;
|
||
return;
|
||
}
|
||
|
||
int objNum_v = (int)rodArcTrees_v.size();
|
||
int objNum_h = (int)rodArcTrees_h.size();
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
for (int j = 0; j < linePtNum; j++)
|
||
{
|
||
scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
|
||
}
|
||
}
|
||
//置标志，用于debug
|
||
for (int i = 0; i < objNum_v; i++)
|
||
{
|
||
int nodeNum = (int)rodArcTrees_v[i].treeNodes.size();
|
||
for (int j = 0; j < nodeNum; j++)
|
||
{
|
||
int lineIdx = rodArcTrees_v[i].treeNodes[j].lineIdx;
|
||
int centerPtIdx = rodArcTrees_v[i].treeNodes[j].peakPtIdx;
|
||
for (int m = rodArcTrees_v[i].treeNodes[j].startPtIdx; m <= rodArcTrees_v[i].treeNodes[j].endPtIdx; m++)
|
||
scanLines[lineIdx][m].nPointIdx = 1;
|
||
scanLines[lineIdx][centerPtIdx].nPointIdx |= 0x10;
|
||
}
|
||
}
|
||
//置标志，用于debug
|
||
for (int i = 0; i < objNum_h; i++)
|
||
{
|
||
int nodeNum = (int)rodArcTrees_h[i].treeNodes.size();
|
||
for (int j = 0; j < nodeNum; j++)
|
||
{
|
||
int ptIdx = rodArcTrees_h[i].treeNodes[j].lineIdx;
|
||
int centerLineIdx = rodArcTrees_h[i].treeNodes[j].peakPtIdx;
|
||
for (int m = rodArcTrees_h[i].treeNodes[j].startPtIdx; m <= rodArcTrees_h[i].treeNodes[j].endPtIdx; m++)
|
||
scanLines[m][ptIdx].nPointIdx |= 2;
|
||
scanLines[centerLineIdx][ptIdx].nPointIdx |= 0x20;
|
||
}
|
||
}
|
||
}
|