4489 lines
128 KiB
C++
4489 lines
128 KiB
C++
#include <vector>
|
||
#include "SG_baseDataType.h"
|
||
#include "SG_baseAlgo_Export.h"
|
||
#include "workpieceHolePositioning_Export.h"
|
||
#include <opencv2/opencv.hpp>
|
||
#include <limits>
|
||
|
||
#define _DEBUG_OUTPUT
|
||
|
||
//version 1.0.0 : base version release to customer
|
||
//version 1.0.2 : 添加了工件姿态（欧拉角输出)
|
||
//version 1.1.0 : c对工件姿态规范化为中心点（操作点）加三个方向矢量
|
||
//version 1.2.0 : 算法完成了6轴验证
|
||
//version 1.3.0 : (1)算法进行了迭代 (2)对结果进行了分层和排序，输出最上层目标
|
||
//version 1.4.0 : 添加了华航孔定位功能
|
||
//version 1.4.1 : 华航孔定位客户发布初始版本，修正了1.4.0版本的一些问题
|
||
//version 1.4.2 : 华航孔定位改进：（1）法向量计算改进（2）添加3x3平滑（3）修正了bug
|
||
//version 1.4.3 : 对1.3.0拓普发孔定位中调平Bug进行了修正
|
||
//version 1.4.4 : 对1.4.3一个小的修正， 不影响结果
|
||
//version 1.4.5 : 添加异物检测，通过errCode输出“有异物”和“无产品”结果
|
||
//version 1.4.6 : 修正问题：4个孔组成工作时，需要4个孔的高度基本一致。
|
||
//version 1.4.7 : 修正问题：在进行异物检测时，计算ZSliceTh时。添加Z计算的保护，防止异常值混入
|
||
//version 1.4.8 : 将工件法向调整为垂直于工件表面
|
||
//version 1.4.9 : 修正工件法向计算的一个Bug
|
||
//version 1.5.0 : 修正工件Y方向矢量计算的一个Bug（不在工件表面平面内）
|
||
//version 1.5.1 : 异物检测时，检测高度由工件最小Z向上1.5mm调整为工件最小Z向上工件高度的一半，工件倾斜时容错性能更好。
|
||
//version 1.5.2 : 修正工件判断中的一个逻辑错误。
|
||
//version 1.5.3 : 添加郑州微力砂轮盘和轮盘架定位API。
|
||
//version 1.5.4 : 添加山东本事机电轮胎定位API。
|
||
//version 1.5.5 : 山东本事机电轮胎定位。（1）添加了轮胎内径大小的输出， （2）按高度排序。最上面的目标排在第一个。
|
||
//version 1.5.6 : 山东本事机电轮胎定位:优化了轮胎定位算法，使用圆扫描提取轮廓点。
|
||
std::string m_strVersion = "HolePostion 1.5.6";
|
||
const char* wd_workpieceHolePositioningVersion(void)
|
||
{
|
||
return m_strVersion.c_str();
|
||
}
|
||
|
||
#ifdef _DEBUG_OUTPUT
|
||
// 二维 int vector (灰度像素 0~255) 转 Mat
|
||
cv::Mat vec2Int2VisMat(const std::vector<std::vector<int>>& data)
|
||
{
|
||
if (data.empty())
|
||
return cv::Mat();
|
||
|
||
int rows = data.size();
|
||
int cols = data[0].size();
|
||
cv::Mat mat(rows, cols, CV_32FC1);
|
||
|
||
// 填充浮点数据
|
||
for (int i = 0; i < rows; i++)
|
||
{
|
||
float* ptr = mat.ptr<float>(i);
|
||
for (int j = 0; j < cols; j++)
|
||
{
|
||
ptr[j] = (float)data[i][j];
|
||
}
|
||
}
|
||
|
||
// 归一化到 0~255 用于显示
|
||
cv::Mat vis;
|
||
cv::normalize(mat, vis, 0, 255, cv::NORM_MINMAX, CV_8UC1);
|
||
return vis;
|
||
}
|
||
// 二维 float vector (深度/浮点数据) 转可视化灰度图
|
||
cv::Mat vec2Float2VisMat(const std::vector<std::vector<double>>& data)
|
||
{
|
||
if (data.empty())
|
||
return cv::Mat();
|
||
|
||
int rows = data.size();
|
||
int cols = data[0].size();
|
||
cv::Mat mat(rows, cols, CV_32FC1);
|
||
|
||
// 填充浮点数据
|
||
for (int i = 0; i < rows; i++)
|
||
{
|
||
float* ptr = mat.ptr<float>(i);
|
||
for (int j = 0; j < cols; j++)
|
||
{
|
||
ptr[j] = (float)data[i][j];
|
||
}
|
||
}
|
||
|
||
// 归一化到 0~255 用于显示
|
||
cv::Mat vis;
|
||
cv::normalize(mat, vis, 0, 255, cv::NORM_MINMAX, CV_8UC1);
|
||
return vis;
|
||
}
|
||
// 二维 方差 (深度/浮点数据) 转可视化灰度图
|
||
cv::Mat vec2Var2VisMat(const std::vector<std::vector<SSG_meanVar>>& data)
|
||
{
|
||
if (data.empty())
|
||
return cv::Mat();
|
||
|
||
int rows = data.size();
|
||
int cols = data[0].size();
|
||
cv::Mat mat(rows, cols, CV_32FC1);
|
||
|
||
// 填充浮点数据
|
||
for (int i = 0; i < rows; i++)
|
||
{
|
||
float* ptr = mat.ptr<float>(i);
|
||
for (int j = 0; j < cols; j++)
|
||
{
|
||
ptr[j] = (float)data[i][j].var;
|
||
}
|
||
}
|
||
|
||
// 归一化到 0~255 用于显示
|
||
cv::Mat vis;
|
||
cv::normalize(mat, vis, 0, 255, cv::NORM_MINMAX, CV_8UC1);
|
||
return vis;
|
||
}
|
||
// 二维 方差 (深度/浮点数据) 转可视化灰度图
|
||
cv::Mat vec2VarAdd2VisMat(const std::vector<std::vector<SSG_meanVar>>& data1, const std::vector<std::vector<SSG_meanVar>>& data2)
|
||
{
|
||
if ((data1.empty())|| (data2.empty()))
|
||
return cv::Mat();
|
||
|
||
int rows = data1.size();
|
||
int cols = data1[0].size();
|
||
if( (data2.size() != rows) || (data2[0].size() != cols))
|
||
return cv::Mat();
|
||
|
||
cv::Mat mat(rows, cols, CV_32FC1);
|
||
// 填充浮点数据
|
||
for (int i = 0; i < rows; i++)
|
||
{
|
||
float* ptr = mat.ptr<float>(i);
|
||
for (int j = 0; j < cols; j++)
|
||
{
|
||
ptr[j] = (float)(data1[i][j].var + data2[i][j].var);
|
||
}
|
||
}
|
||
|
||
// 归一化到 0~255 用于显示
|
||
cv::Mat vis;
|
||
cv::normalize(mat, vis, 0, 255, cv::NORM_MINMAX, CV_8UC1);
|
||
return vis;
|
||
}
|
||
#endif
|
||
|
||
//相机水平安装计算地面调平参数。
|
||
//相机Z轴基本平行地面时，需要以地面为参照，将相机调水平
|
||
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
|
||
SSG_planeCalibPara wd_getGroundCalibPara(
|
||
std::vector< std::vector<SVzNL3DPosition>>& scanLines)
|
||
{
|
||
return sg_getPlaneCalibPara2(scanLines);
|
||
}
|
||
|
||
//相机水平时姿态调平，并去除地面
|
||
void wd_lineDataR(
|
||
std::vector< SVzNL3DPosition>& a_line,
|
||
const double* camPoseR,
|
||
double groundH)
|
||
{
|
||
lineDataRT_vector(a_line, camPoseR, groundH);
|
||
}
|
||
|
||
SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, const 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 _getLinePositions(SVzNL3DPoint& seed1, SVzNL3DPoint& seed2, SVzNLRect& roi2D, SVzNLRangeD& distRange, std::vector<SVzNL2DPoint>& pts)
|
||
{
|
||
double len = sqrt(pow(seed2.x - seed1.x, 2) + pow(seed2.y - seed1.y, 2));
|
||
int x0 = (int)seed1.x - roi2D.left;
|
||
int y0 = (int)seed1.y - roi2D.top;
|
||
|
||
int x1 = (int)seed2.x - roi2D.left;
|
||
int y1 = (int)seed2.y - roi2D.top;
|
||
|
||
std::vector<SVzNL2DPoint> pts_all;
|
||
drawLine(x0, y0, x1, y1, pts_all);
|
||
|
||
for (int i = 0; i < (int)pts_all.size(); i++)
|
||
{
|
||
double data = double((pts_all[i].x - x0) * (pts_all[i].x - x0) + (pts_all[i].y - y0) * (pts_all[i].y - y0));
|
||
double dist = sqrt(data);
|
||
double dist_k = dist / len;
|
||
if ((dist_k >= distRange.min) && (dist_k <= distRange.max))
|
||
pts.push_back(pts_all[i]);
|
||
}
|
||
}
|
||
|
||
void _getLinePoints(
|
||
std::vector<std::vector<double>>& quantiValue,
|
||
SVzNL3DPoint& seed1, SVzNL3DPoint& seed2,
|
||
SVzNLRect& roi2D, SVzNLRangeD& distRange,
|
||
std::vector<cv::Point3d>& Points3ds)
|
||
{
|
||
std::vector<SVzNL2DPoint> pts_2d;
|
||
_getLinePositions(seed1, seed2, roi2D, distRange, pts_2d);
|
||
for (int i = 0; i < (int)pts_2d.size(); i++)
|
||
{
|
||
cv::Point3d a_pt3d;
|
||
a_pt3d.x = (double)(pts_2d[i].x + roi2D.left) + 0.5;
|
||
a_pt3d.y = (double)(pts_2d[i].y + roi2D.top) + 0.5;
|
||
a_pt3d.z = quantiValue[pts_2d[i].x][pts_2d[i].y];
|
||
if (a_pt3d.z > 1e-4)
|
||
Points3ds.push_back(a_pt3d);
|
||
}
|
||
}
|
||
|
||
bool _validateLineZChange(
|
||
std::vector<std::vector<double>>& quantiValue,
|
||
SVzNL3DPoint& seed1, SVzNL3DPoint& seed2,
|
||
SVzNLRect& roi2D, SVzNLRangeD& distRange,
|
||
double maxZChange
|
||
)
|
||
{
|
||
std::vector<cv::Point3d> Points3ds;
|
||
_getLinePoints(
|
||
quantiValue,
|
||
seed1, seed2,
|
||
roi2D, distRange,
|
||
Points3ds);
|
||
|
||
double refZ = (seed1.z + seed2.z) / 2;
|
||
for (int i = 0; i < (int)Points3ds.size(); i++)
|
||
{
|
||
double zDiff = abs(Points3ds[i].z - refZ);
|
||
if (zDiff > maxZChange)
|
||
return false;
|
||
}
|
||
return true;
|
||
}
|
||
|
||
//搜索最接近distance的目标
|
||
int distanceSearchObject(
|
||
SVzNL3DPoint seed,
|
||
std::vector<SWD_HoleInfo>& holes,
|
||
double distance, double distDeviation, double dirAngle, double deltaZ,
|
||
std::vector<std::vector<double>>& quantiValue,
|
||
SVzNLRect& roi2D,
|
||
SVzNLRangeD& distRange)
|
||
{
|
||
int result = -1;
|
||
int holeSize = (int)holes.size();
|
||
double minDistDiff = DBL_MAX;
|
||
int minDistIndex = -1;
|
||
for (int i = 0; i < holeSize; i++)
|
||
{
|
||
if (holes[i].radius < 0)
|
||
continue;
|
||
|
||
double dist = sqrt(pow(seed.x - holes[i].center.x, 2) + pow(seed.y - holes[i].center.y, 2));
|
||
double distDiff = abs(dist - distance);
|
||
double zDiff = abs(seed.z - holes[i].center.z);
|
||
double angle = atan2(seed.y - holes[i].center.y, seed.x - holes[i].center.x);
|
||
angle = angle * 180.0 / PI;
|
||
double angleDiff = computeAngleDiff(angle, dirAngle);
|
||
if (angleDiff > 90)
|
||
angleDiff = 180 - angleDiff;
|
||
|
||
if ((zDiff < deltaZ) && (angleDiff < 45))
|
||
{
|
||
if (minDistDiff > distDiff)
|
||
{
|
||
//添加距离检测
|
||
bool isValid = _validateLineZChange(
|
||
quantiValue,
|
||
seed, holes[i].center,
|
||
roi2D, distRange,
|
||
deltaZ
|
||
);
|
||
if (true == isValid)
|
||
{
|
||
minDistDiff = distDiff;
|
||
minDistIndex = i;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
if ((minDistIndex >= 0) && (minDistDiff < distDeviation))
|
||
result = minDistIndex;
|
||
return result;
|
||
}
|
||
|
||
//搜索最接近distance且角度为angle的目标, 以角度为优先
|
||
int angleConditionDistanceSearch(
|
||
SVzNL3DPoint seed, SVzNL3DPoint angleSide,
|
||
std::vector<SWD_HoleInfo>& holes,
|
||
double distance, double distDeviation, double deltaZ,
|
||
SVzNLRangeD angleRange,
|
||
std::vector<std::vector<double>>& quantiValue,
|
||
SVzNLRect& roi2D,
|
||
SVzNLRangeD& distRange)
|
||
{
|
||
int result = -1;
|
||
int holeSize = (int)holes.size();
|
||
std::vector< int> distValidHoleIndex;
|
||
for (int i = 0; i < holeSize; i++)
|
||
{
|
||
if (holes[i].radius < 0)
|
||
continue;
|
||
|
||
double dist = sqrt(pow(seed.x - holes[i].center.x, 2) + pow(seed.y - holes[i].center.y, 2));
|
||
double distDiff = abs(dist - distance);
|
||
double zDiff = abs(seed.z - holes[i].center.z);
|
||
if ( (distDiff < distDeviation) && (zDiff < deltaZ))
|
||
{
|
||
distValidHoleIndex.push_back(i);
|
||
}
|
||
}
|
||
if (distValidHoleIndex.size() == 1)
|
||
{
|
||
int idx = distValidHoleIndex[0];
|
||
double angle = computeXOYVertexAngle(seed, angleSide, holes[idx].center);
|
||
if( (angle >= angleRange.min) &&(angle <= angleRange.max))
|
||
result = idx;
|
||
}
|
||
else if (distValidHoleIndex.size() > 1)
|
||
{
|
||
double bestAngle = (angleRange.min + angleRange.max) / 2;
|
||
double minAngleDeviateion = DBL_MAX;
|
||
int minAngleIdx = -1;
|
||
for (int i = 0, i_max = (int)distValidHoleIndex.size(); i < i_max; i++)
|
||
{
|
||
int idx = distValidHoleIndex[i];
|
||
double angle = computeXOYVertexAngle(seed, angleSide, holes[idx].center);
|
||
if ((angle >= angleRange.min) && (angle <= angleRange.max))
|
||
{
|
||
double angleDiff = abs(angle - bestAngle);
|
||
if (minAngleDeviateion > angleDiff)
|
||
{
|
||
//添加距离检测
|
||
bool isValid = _validateLineZChange(
|
||
quantiValue,
|
||
seed, holes[idx].center,
|
||
roi2D, distRange,
|
||
deltaZ
|
||
);
|
||
if (true == isValid)
|
||
{
|
||
minAngleDeviateion = angleDiff;
|
||
minAngleIdx = idx;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
result = minAngleIdx;
|
||
}
|
||
return result;
|
||
}
|
||
|
||
double _getMeanZ(std::vector<std::vector<double>>& quantiValue, SVzNL3DPoint seed, SVzNLRect& roi2D, double rectR, double zRange)
|
||
{
|
||
int cols = (int)quantiValue.size();
|
||
int rows = (int)quantiValue[0].size();
|
||
|
||
int px = (int)seed.x - roi2D.left;
|
||
int py = (int)seed.y - roi2D.top;
|
||
int win = (int)rectR;
|
||
int hist = 0;
|
||
double zSum = 0;
|
||
double refZ = seed.z;
|
||
for (int i = -win; i <= win; i++)
|
||
{
|
||
for (int j = -win; j <= win; j++)
|
||
{
|
||
int qx = px + i;
|
||
int qy = py + j;
|
||
if ((qx >= 0) && (qx < cols) && (qy >= 0) && (qy < rows))
|
||
{
|
||
if (quantiValue[qx][qy] > 1e-4)
|
||
{
|
||
double zDiff = abs(quantiValue[qx][qy] - refZ);
|
||
if (zDiff < zRange)
|
||
{
|
||
zSum += quantiValue[qx][qy];
|
||
hist++;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
if (hist == 0)
|
||
return 0;
|
||
else
|
||
return (zSum / hist);
|
||
}
|
||
|
||
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;
|
||
}
|
||
|
||
#if 0
|
||
void _updateROI(SSG_ROIRectD& roi, SVzNL3DPoint& a_pt)
|
||
{
|
||
if (a_pt.z > 1E-4)
|
||
{
|
||
if (roi.left < 0)
|
||
{
|
||
roi.left = a_pt.x;
|
||
roi.right = a_pt.x;
|
||
roi.left = a_pt.y;
|
||
roi.right = a_pt.y;
|
||
}
|
||
else
|
||
{
|
||
if (roi.left > a_pt.x)
|
||
roi.left = a_pt.x;
|
||
if (roi.right < a_pt.x)
|
||
roi.right = a_pt.x;
|
||
if (roi.top > a_pt.y)
|
||
roi.top = a_pt.y;
|
||
if (roi.bottom < a_pt.y)
|
||
roi.bottom = a_pt.y;
|
||
}
|
||
}
|
||
return;
|
||
}
|
||
|
||
void _updateRenge(SVzNLRange& range, int idx)
|
||
{
|
||
if (range.nMin < 0)
|
||
{
|
||
range.nMin = idx;
|
||
range.nMax = idx;
|
||
}
|
||
else
|
||
{
|
||
if (range.nMin > idx)
|
||
range.nMin = idx;
|
||
if (range.nMax < idx)
|
||
range.nMax = idx;
|
||
}
|
||
return;
|
||
}
|
||
|
||
bool _checkExist(int id, std::vector<int>& buff)
|
||
{
|
||
for (int i = 0; i < (int)buff.size(); i++)
|
||
{
|
||
if (id == buff[i])
|
||
return true;
|
||
}
|
||
return false;
|
||
}
|
||
|
||
void _searchNeighbours(
|
||
int selfId, int chkExtening,
|
||
int sLineIdx, int eLineIdx,
|
||
SVzNLRange ptIdxRange,
|
||
std::vector<std::vector<int>>& treeMask,
|
||
std::vector<int>& neighbours)
|
||
{
|
||
int lineNum = (int)treeMask.size();
|
||
int ptNum = treeMask[0].size();
|
||
for (int line = sLineIdx - chkExtening; line <= eLineIdx+ chkExtening; line++)
|
||
{
|
||
if ((line >= 0) && (line < lineNum))
|
||
{
|
||
for (int ptIdx = ptIdxRange.nMin - chkExtening; ptIdx <= ptIdxRange.nMax + chkExtening; ptIdx++)
|
||
{
|
||
if ((ptIdx >= 0) && (ptIdx < ptNum))
|
||
{
|
||
if ((treeMask[line][ptIdx] >= 0) && (treeMask[line][ptIdx] != selfId))
|
||
{
|
||
bool isExist = _checkExist(treeMask[line][ptIdx], neighbours);
|
||
if (false == isExist)
|
||
neighbours.push_back(treeMask[line][ptIdx]);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
return;
|
||
}
|
||
|
||
void WD_getHoleInfo(
|
||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||
const SSG_lineSegParam lineSegPara,
|
||
const SSG_outlierFilterParam filterParam,
|
||
const SSG_treeGrowParam growParam,
|
||
const double valieCommonNumRatio,
|
||
std::vector<SWD_segFeatureTree>& segTrees_v,
|
||
std::vector<SWD_segFeatureTree>& segTrees_h,
|
||
std::vector<SSG_intPair>& validObjects
|
||
)
|
||
{
|
||
int lineNum = (int)scanLines.size();
|
||
int linePtNum = (int)scanLines[0].size();
|
||
|
||
std::vector<std::vector<int>> pointMask;
|
||
pointMask.resize(lineNum);
|
||
|
||
//提取空白线段特征（孔特征）
|
||
std::vector<std::vector<SWD_segFeature>> holeGaps;
|
||
//提取线段端点特征
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
if (line == 1047)
|
||
int kkk = 1;
|
||
|
||
std::vector<SVzNL3DPosition>& lineData = scanLines[line];
|
||
pointMask[line].resize(lineData.size());
|
||
std::fill(pointMask[line].begin(), pointMask[line].end(), 0);//初始化为0
|
||
//滤波，滤除异常点
|
||
sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam);
|
||
|
||
std::vector<SSG_RUN> segs;
|
||
wd_getLineDataNullIntervals(lineData, lineSegPara, segs);
|
||
|
||
//将seg端点作为边缘点。做了地面调平后，垂直孔的内侧在XY平面上均为边缘点。
|
||
std::vector<SWD_segFeature> line_gaps;
|
||
for (int i = 0, i_max = (int)segs.size(); i < i_max; i++)
|
||
{
|
||
int ptIdx_1 = segs[i].start;
|
||
int ptIdx_2 = segs[i].start + segs[i].len - 1;
|
||
SWD_segFeature a_gap;
|
||
a_gap.lineIdx = line;
|
||
a_gap.startPtIdx = ptIdx_1;
|
||
a_gap.endPtIdx = ptIdx_2;
|
||
a_gap.startPt = lineData[ptIdx_1].pt3D;
|
||
a_gap.endPt = lineData[ptIdx_2].pt3D;
|
||
a_gap.featureValue = abs(a_gap.startPt.y - a_gap.endPt.y);
|
||
line_gaps.push_back(a_gap);
|
||
}
|
||
holeGaps.push_back(line_gaps);
|
||
}
|
||
//特征生长
|
||
wd_getSegFeatureGrowingTrees_2(holeGaps, segTrees_v, growParam);
|
||
|
||
//生成水平扫描
|
||
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_segFeature>> holeGaps_h;
|
||
for (int line = 0; line < linePtNum; line++)
|
||
{
|
||
if (line == 974)
|
||
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_RUN> segs;
|
||
wd_getLineDataNullIntervals(lineData, lineSegPara, segs);
|
||
|
||
//将seg端点作为边缘点。做了地面调平后，垂直孔的内侧在XY平面上均为边缘点。
|
||
std::vector<SWD_segFeature> line_gaps;
|
||
for (int i = 0, i_max = (int)segs.size(); i < i_max; i++)
|
||
{
|
||
int ptIdx_1 = segs[i].start;
|
||
int ptIdx_2 = segs[i].start + segs[i].len - 1;
|
||
SWD_segFeature a_gap;
|
||
a_gap.lineIdx = line;
|
||
a_gap.startPtIdx = ptIdx_1;
|
||
a_gap.endPtIdx = ptIdx_2;
|
||
a_gap.startPt = lineData[ptIdx_1].pt3D;
|
||
a_gap.endPt = lineData[ptIdx_2].pt3D;
|
||
a_gap.featureValue = abs(a_gap.startPt.y - a_gap.endPt.y);
|
||
line_gaps.push_back(a_gap);
|
||
}
|
||
holeGaps_h.push_back(line_gaps);
|
||
}
|
||
//特征生长
|
||
wd_getSegFeatureGrowingTrees_2(holeGaps_h, segTrees_h, growParam);
|
||
|
||
//创建Tree所在孔洞的Mask
|
||
std::vector<std::vector<int>> treeMask_v;
|
||
treeMask_v.resize(lineNum);
|
||
std::vector<std::vector<int>> treeMask_h;
|
||
treeMask_h.resize(lineNum);
|
||
for (int i = 0; i < lineNum; i++)
|
||
{
|
||
treeMask_v[i].resize(linePtNum);
|
||
std::fill(treeMask_v[i].begin(), treeMask_v[i].end(), -1);
|
||
treeMask_h[i].resize(linePtNum);
|
||
std::fill(treeMask_h[i].begin(), treeMask_h[i].end(), -1);
|
||
}
|
||
//标注
|
||
std::vector<SSG_treeInfo> treeInfo_v;
|
||
treeInfo_v.resize(segTrees_v.size());
|
||
for (int i = 0; i < (int)segTrees_v.size(); i++)
|
||
{
|
||
SWD_segFeatureTree& a_tree = segTrees_v[i];
|
||
treeInfo_v[i].treeIdx = i;
|
||
treeInfo_v[i].sLineIdx = a_tree.sLineIdx;
|
||
treeInfo_v[i].eLineIdx = a_tree.eLineIdx;
|
||
treeInfo_v[i].vTreeFlag = 0;
|
||
treeInfo_v[i].treeType = 0;
|
||
treeInfo_v[i].roi = { -1, -1, -1, -1 };
|
||
int nullPtSize = 0;
|
||
SSG_ROIRectD roi = { -1, -1, -1, -1 };
|
||
SVzNLRange ptIdxRange = { -1, -1 };
|
||
if (a_tree.treeNodes.size() > 0)
|
||
{
|
||
for (int m = 0; m < (int)a_tree.treeNodes.size(); m++)
|
||
{
|
||
SWD_segFeature& a_seg = a_tree.treeNodes[m];
|
||
for (int n = a_seg.startPtIdx; n <= a_seg.endPtIdx; n++)
|
||
treeMask_v[a_seg.lineIdx][n] = i;
|
||
|
||
nullPtSize += a_seg.endPtIdx - a_seg.startPtIdx + 1;
|
||
_updateROI(roi, scanLines[a_seg.lineIdx][a_seg.startPtIdx].pt3D);
|
||
_updateROI(roi, scanLines[a_seg.lineIdx][a_seg.endPtIdx].pt3D);
|
||
_updateRenge(ptIdxRange, a_seg.startPtIdx);
|
||
_updateRenge(ptIdxRange, a_seg.endPtIdx);
|
||
//scanLinesInput[a_seg.lineIdx][a_seg.endPtIdx].nPointIdx = 0x01;
|
||
//scanLinesInput[a_seg.lineIdx][a_seg.startPtIdx].nPointIdx = 0x01;
|
||
}
|
||
treeInfo_v[i].treeType = nullPtSize;
|
||
treeInfo_v[i].roi = roi;
|
||
treeInfo_v[i].ptIdxRange = ptIdxRange;
|
||
}
|
||
}
|
||
std::vector<SSG_treeInfo> treeInfo_h;
|
||
treeInfo_h.resize(segTrees_h.size());
|
||
for (int i = 0; i < (int)segTrees_h.size(); i++)
|
||
{
|
||
SWD_segFeatureTree& a_tree = segTrees_h[i];
|
||
treeInfo_h[i].treeIdx = i;
|
||
treeInfo_h[i].sLineIdx = a_tree.sLineIdx;
|
||
treeInfo_h[i].eLineIdx = a_tree.eLineIdx;
|
||
treeInfo_h[i].vTreeFlag = 0;
|
||
treeInfo_h[i].treeType = 0;
|
||
treeInfo_h[i].roi = { -1, -1, -1, -1 };
|
||
int nullPtSize = 0;
|
||
SSG_ROIRectD roi = { -1, -1, -1, -1 };
|
||
SVzNLRange ptIdxRange = { -1, -1 };
|
||
if (a_tree.treeNodes.size() > 0)
|
||
{
|
||
for (int m = 0; m < (int)a_tree.treeNodes.size(); m++)
|
||
{
|
||
SWD_segFeature& a_seg = a_tree.treeNodes[m];
|
||
for (int n = a_seg.startPtIdx; n <= a_seg.endPtIdx; n++)
|
||
treeMask_h[n][a_seg.lineIdx] = i;
|
||
|
||
nullPtSize += a_seg.endPtIdx - a_seg.startPtIdx + 1;
|
||
_updateROI(roi, scanLines[a_seg.startPtIdx][a_seg.lineIdx].pt3D);
|
||
_updateROI(roi, scanLines[a_seg.endPtIdx][a_seg.lineIdx].pt3D);
|
||
_updateRenge(ptIdxRange, a_seg.startPtIdx);
|
||
_updateRenge(ptIdxRange, a_seg.endPtIdx);
|
||
//scanLinesInput[a_seg.startPtIdx][a_seg.lineIdx].nPointIdx |= 0x02;
|
||
//scanLinesInput[a_seg.endPtIdx][a_seg.lineIdx].nPointIdx |= 0x02;
|
||
}
|
||
treeInfo_h[i].treeType = nullPtSize;
|
||
treeInfo_h[i].roi = roi;
|
||
treeInfo_h[i].ptIdxRange = ptIdxRange;
|
||
}
|
||
}
|
||
//水平和垂直目标合并
|
||
int vTreeSize = (int)segTrees_v.size();
|
||
int hTreeSize = (int)segTrees_h.size();
|
||
std::vector<std::vector<int>> treeHVInfo; //统计垂直和水平的tree的位置信息
|
||
treeHVInfo.resize(vTreeSize);
|
||
for (int i = 0; i < vTreeSize; i++)
|
||
{
|
||
treeHVInfo[i].resize(hTreeSize);
|
||
std::fill(treeHVInfo[i].begin(), treeHVInfo[i].end(), 0);
|
||
}
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
for (int ptIdx = 0; ptIdx < linePtNum; ptIdx++)
|
||
{
|
||
int idx_v = treeMask_v[line][ptIdx];
|
||
int idx_h = treeMask_h[line][ptIdx];
|
||
if ((idx_v >= 0) && (idx_h >= 0))
|
||
treeHVInfo[idx_v][idx_h]++;
|
||
}
|
||
}
|
||
|
||
//生成候选目标
|
||
std::vector<SSG_intPair> objects;
|
||
for (int i = 0; i < vTreeSize; i++)
|
||
{
|
||
SWD_segFeatureTree& a_tree = segTrees_v[i];
|
||
if ((a_tree.sLineIdx <= 1047) && (a_tree.eLineIdx >= 1047))
|
||
int kkk = 1;
|
||
|
||
int totalSize_v = treeInfo_v[i].treeType;
|
||
int commonSize = 0;
|
||
int hTreeIdx = -1;
|
||
for (int j = 0; j < hTreeSize; j++)
|
||
{
|
||
if (commonSize < treeHVInfo[i][j])
|
||
{
|
||
hTreeIdx = j;
|
||
commonSize = treeHVInfo[i][j];
|
||
}
|
||
}
|
||
if (hTreeIdx >= 0)
|
||
{
|
||
int totalSize_h = treeInfo_h[hTreeIdx].treeType;
|
||
int sizeV_th = (int)((double)totalSize_v * valieCommonNumRatio);
|
||
int sizeH_th = (int)((double)totalSize_h * valieCommonNumRatio);
|
||
if ((commonSize > sizeH_th) && (commonSize > sizeV_th))
|
||
{
|
||
SSG_intPair a_obj;
|
||
a_obj.data_0 = i;
|
||
a_obj.data_1 = hTreeIdx;
|
||
a_obj.idx = commonSize;
|
||
objects.push_back(a_obj);
|
||
treeInfo_v[i].data = commonSize;
|
||
treeInfo_h[hTreeIdx].data = commonSize;
|
||
}
|
||
}
|
||
}
|
||
|
||
//滤除相邻。每个目标保留一个
|
||
for (int i = 0; i < (int)objects.size(); i++)
|
||
{
|
||
int vTreeIdx = objects[i].data_0;
|
||
if (treeInfo_v[vTreeIdx].vTreeFlag < 0)
|
||
continue;
|
||
|
||
std::vector<int> neighbours;
|
||
_searchNeighbours(vTreeIdx, 3,
|
||
treeInfo_v[vTreeIdx].sLineIdx, treeInfo_v[vTreeIdx].eLineIdx,
|
||
treeInfo_v[vTreeIdx].ptIdxRange, treeMask_v, neighbours);
|
||
|
||
int bestIdx = vTreeIdx;
|
||
int maxValue = treeInfo_v[vTreeIdx].data;
|
||
if (neighbours.size() > 0)
|
||
{
|
||
for (int j = 0; j < (int)neighbours.size(); j++)
|
||
{
|
||
int idx = neighbours[j];
|
||
if (maxValue < treeInfo_v[idx].data)
|
||
{
|
||
maxValue = treeInfo_v[idx].data;
|
||
bestIdx = idx;
|
||
}
|
||
}
|
||
if (bestIdx != vTreeIdx)
|
||
treeInfo_v[vTreeIdx].vTreeFlag = -1;
|
||
for (int j = 0; j < (int)neighbours.size(); j++)
|
||
{
|
||
int idx = neighbours[j];
|
||
if (bestIdx != idx)
|
||
treeInfo_v[idx].vTreeFlag = -1;
|
||
}
|
||
}
|
||
}
|
||
|
||
for (int i = 0; i < (int)objects.size(); i++)
|
||
{
|
||
int vTreeIdx = objects[i].data_0;
|
||
if (treeInfo_v[vTreeIdx].vTreeFlag < 0)
|
||
continue;
|
||
|
||
validObjects.push_back(objects[i]);
|
||
}
|
||
}
|
||
#endif
|
||
|
||
bool _compareByXValue(WD_workpieceInfo& a, WD_workpieceInfo& b)
|
||
{
|
||
return a.center.x < b.center.x;
|
||
}
|
||
void _getYTopLine(
|
||
std::vector< WD_workpieceInfo>& workpieceSrc,
|
||
std::vector< WD_workpieceInfo>& firstLine,
|
||
std::vector< WD_workpieceInfo>& restWorkpiece,
|
||
double yLayerTh)
|
||
{
|
||
//搜索Y最小值
|
||
if (workpieceSrc.size() == 0)
|
||
return;
|
||
|
||
double minY = workpieceSrc[0].center.y;
|
||
for (int i = 1; i < (int)workpieceSrc.size(); i++)
|
||
{
|
||
if (minY > workpieceSrc[i].center.y)
|
||
minY = workpieceSrc[i].center.y;
|
||
}
|
||
double topLayerTh = minY + yLayerTh;
|
||
for (int i = 0; i < (int)workpieceSrc.size(); i++)
|
||
{
|
||
if (workpieceSrc[i].center.y < topLayerTh)
|
||
firstLine.push_back(workpieceSrc[i]);
|
||
else
|
||
restWorkpiece.push_back(workpieceSrc[i]);
|
||
}
|
||
std::sort(firstLine.begin(), firstLine.end(), _compareByXValue);
|
||
return;
|
||
}
|
||
|
||
SSG_ROIRectD _getClusterROI(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;
|
||
}
|
||
|
||
double _getMinZ(std::vector<cv::Point3d>& Points3ds)
|
||
{
|
||
double minZ = -1;
|
||
for (int i = 0; i < (int)Points3ds.size(); i++)
|
||
{
|
||
if ((minZ < 0) || (minZ > Points3ds[i].z))
|
||
minZ = Points3ds[i].z;
|
||
}
|
||
return minZ;
|
||
}
|
||
|
||
//工件孔定位-拓普发工件孔定位
|
||
void wd_workpieceHolePositioning(
|
||
std::vector< std::vector<SVzNL3DPosition>>& scanLinesInput,
|
||
const WD_workpieceHoleParam workpiecePara,
|
||
const SSG_lineSegParam lineSegPara,
|
||
const SSG_outlierFilterParam filterParam,
|
||
const SSG_treeGrowParam growParam,
|
||
const SSG_planeCalibPara groundCalibPara,
|
||
std::vector< WD_workpieceInfo>& workpiecePositioning,
|
||
int* errCode)
|
||
{
|
||
*errCode = 0;
|
||
|
||
int lineNum = (int)scanLinesInput.size();
|
||
std::vector< std::vector<SVzNL3DPosition>> scanLines;
|
||
scanLines.resize(lineNum);
|
||
int linePtNum = (int)scanLinesInput[0].size();
|
||
bool isGridData = true;
|
||
for (int i = 0; i < lineNum; i++)
|
||
{
|
||
if (linePtNum != (int)scanLinesInput[i].size())
|
||
isGridData = false;
|
||
|
||
scanLines[i].resize(scanLinesInput[i].size());
|
||
std::copy(scanLinesInput[i].begin(), scanLinesInput[i].end(), scanLines[i].begin()); // 使用std::copy算法
|
||
|
||
for (int j = 0; j < (int)scanLinesInput[i].size(); j++)
|
||
scanLinesInput[i][j].nPointIdx = 0; //清零，用于debug时记录信息
|
||
}
|
||
if (false == isGridData)//数据不是网格格式
|
||
{
|
||
*errCode = SG_ERR_NOT_GRID_FORMAT;
|
||
return;
|
||
}
|
||
for (int i = 0; i < lineNum; i++)
|
||
{ //行处理
|
||
//调平，去除地面
|
||
wd_lineDataR(scanLines[i], groundCalibPara.planeCalib, -1);
|
||
}
|
||
|
||
//生成量化数据，以1mm为量化尺度，用于确定工件表面高度
|
||
SVzNL3DRangeD roi3D = sg_getScanDataROI_vector(scanLines);
|
||
SVzNLRect roi2D;
|
||
roi2D.left = (int)roi3D.xRange.min;
|
||
roi2D.right = (int)roi3D.xRange.max;
|
||
roi2D.top = (int)roi3D.yRange.min;
|
||
roi2D.bottom = (int)roi3D.yRange.max;
|
||
int quanti_X = roi2D.right - roi2D.left + 1;
|
||
int quanti_Y = roi2D.bottom - roi2D.top + 1;
|
||
std::vector<std::vector<double>> quantiValue;
|
||
std::vector<std::vector<int>> quantiHist;
|
||
quantiValue.resize(quanti_X);
|
||
quantiHist.resize(quanti_X);
|
||
for (int i = 0; i < quanti_X; i++)
|
||
{
|
||
quantiValue[i].resize(quanti_Y);
|
||
std::fill(quantiValue[i].begin(), quantiValue[i].end(), 0);//初始化为0
|
||
quantiHist[i].resize(quanti_Y);
|
||
std::fill(quantiHist[i].begin(), quantiHist[i].end(), 0);//初始化为0
|
||
}
|
||
//以1mm尺度量化
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
for (int j = 0; j < linePtNum; j++)
|
||
{
|
||
SVzNL3DPoint& a_pt = scanLines[line][j].pt3D;
|
||
if (a_pt.z > 1e-4)
|
||
{
|
||
int qx = (int)a_pt.x - roi2D.left;
|
||
int qy = (int)a_pt.y - roi2D.top;
|
||
quantiValue[qx][qy] += a_pt.z;
|
||
quantiHist[qx][qy] += 1;
|
||
}
|
||
}
|
||
}
|
||
for (int ix = 0; ix < quanti_X; ix++)
|
||
{
|
||
for (int iy = 0; iy < quanti_Y; iy++)
|
||
{
|
||
if (quantiHist[ix][iy] > 0)
|
||
quantiValue[ix][iy] = quantiValue[ix][iy] / quantiHist[ix][iy];
|
||
}
|
||
}
|
||
|
||
std::vector<SWD_segFeatureTree> segTrees_v;
|
||
std::vector<SWD_segFeatureTree> segTrees_h;
|
||
std::vector<SSG_intPair> validObjects;
|
||
double valieCommonNumRatio = 0.25;
|
||
WD_getHoleInfo(scanLines, lineSegPara, filterParam, growParam, valieCommonNumRatio, segTrees_v, segTrees_h, validObjects);
|
||
|
||
for (int i = 0; i < lineNum; i++)
|
||
{
|
||
for (int j = 0; j < (int)scanLines[i].size(); j++)
|
||
scanLines[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)segTrees_v[vTreeIdx].treeNodes.size(); m++)
|
||
{
|
||
SWD_segFeature& a_seg = segTrees_v[vTreeIdx].treeNodes[m];
|
||
if (scanLines[a_seg.lineIdx][a_seg.endPtIdx].nPointIdx == 0)
|
||
{
|
||
scanLines[a_seg.lineIdx][a_seg.endPtIdx].nPointIdx = vTreeIdx + 1; // 0x01;
|
||
scanLinesInput[a_seg.lineIdx][a_seg.endPtIdx].nPointIdx = vTreeIdx + 1; // 0x01;
|
||
SVzNL2DPoint a_pos = { a_seg.lineIdx , a_seg.endPtIdx };
|
||
a_cluster.push_back(a_pos);
|
||
_updateRoi3D(a_roi3D, scanLines[a_seg.lineIdx][a_seg.endPtIdx].pt3D);
|
||
}
|
||
if (scanLines[a_seg.lineIdx][a_seg.startPtIdx].nPointIdx == 0)
|
||
{
|
||
scanLines[a_seg.lineIdx][a_seg.startPtIdx].nPointIdx = vTreeIdx + 1; // 0x01;
|
||
scanLinesInput[a_seg.lineIdx][a_seg.startPtIdx].nPointIdx = vTreeIdx + 1; // 0x01;
|
||
SVzNL2DPoint a_pos = { a_seg.lineIdx , a_seg.startPtIdx };
|
||
a_cluster.push_back(a_pos);
|
||
_updateRoi3D(a_roi3D, scanLines[a_seg.lineIdx][a_seg.startPtIdx].pt3D);
|
||
}
|
||
}
|
||
for (int m = 0; m < (int)segTrees_h[hTreeIdx].treeNodes.size(); m++)
|
||
{
|
||
SWD_segFeature& a_seg = segTrees_h[hTreeIdx].treeNodes[m];
|
||
if (scanLines[a_seg.startPtIdx][a_seg.lineIdx].nPointIdx == 0)
|
||
{
|
||
scanLines[a_seg.startPtIdx][a_seg.lineIdx].nPointIdx = vTreeIdx + 1; // 0x02;
|
||
scanLinesInput[a_seg.startPtIdx][a_seg.lineIdx].nPointIdx = vTreeIdx + 1; // 0x02;
|
||
SVzNL2DPoint a_pos = { a_seg.startPtIdx , a_seg.lineIdx };
|
||
a_cluster.push_back(a_pos);
|
||
_updateRoi3D(a_roi3D, scanLines[a_seg.startPtIdx][a_seg.lineIdx].pt3D);
|
||
}
|
||
if (scanLines[a_seg.endPtIdx][a_seg.lineIdx].nPointIdx == 0)
|
||
{
|
||
scanLines[a_seg.endPtIdx][a_seg.lineIdx].nPointIdx = vTreeIdx + 1; // 0x02;
|
||
scanLinesInput[a_seg.endPtIdx][a_seg.lineIdx].nPointIdx = vTreeIdx + 1; // 0x02;
|
||
SVzNL2DPoint a_pos = { a_seg.endPtIdx , a_seg.lineIdx };
|
||
a_cluster.push_back(a_pos);
|
||
_updateRoi3D(a_roi3D, scanLines[a_seg.endPtIdx][a_seg.lineIdx].pt3D);
|
||
}
|
||
}
|
||
clusters.push_back(a_cluster);
|
||
clustersRoi3D.push_back(a_roi3D);
|
||
}
|
||
|
||
//聚类结果分析
|
||
std::vector<int> validCluserIndexing;
|
||
int clusterSize = (int)clusters.size();
|
||
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;
|
||
if ((L > workpiecePara.holeDiameter * 0.5) && (L < workpiecePara.holeDiameter * 2.5) &&
|
||
(W > workpiecePara.holeDiameter * 0.5) && (W < workpiecePara.holeDiameter * 2.5))
|
||
validCluserIndexing.push_back(i);
|
||
}
|
||
//生成结果
|
||
std::vector< SWD_HoleInfo> holes;
|
||
int objectSize = (int)validCluserIndexing.size();
|
||
for (int objIdx = 0; objIdx < objectSize; objIdx++)
|
||
{
|
||
std::vector<SVzNL3DPoint> pointArray;
|
||
int clusterIdx = validCluserIndexing[objIdx];
|
||
//取cluster上的点
|
||
int clusterPtSize = (int)clusters[clusterIdx].size();
|
||
double minZ = DBL_MAX;
|
||
for (int i = 0; i < clusterPtSize; i++)
|
||
{
|
||
SVzNL2DPoint a_pos = clusters[clusterIdx][i];
|
||
int lineIdx = a_pos.x;
|
||
int ptIdx = a_pos.y;
|
||
SVzNL3DPoint a_pt3d = scanLines[lineIdx][ptIdx].pt3D;
|
||
if (minZ > a_pt3d.z)
|
||
minZ = a_pt3d.z;
|
||
pointArray.push_back(a_pt3d);
|
||
}
|
||
//圆拟合
|
||
SVzNL3DPoint center;
|
||
double radius;
|
||
double err = fitCircleByLeastSquare(pointArray, center, radius);
|
||
center.z = minZ;
|
||
SWD_HoleInfo a_hole;
|
||
a_hole.center = { center.x, center.y, center.z };
|
||
a_hole.radius = radius;
|
||
holes.push_back(a_hole);
|
||
}
|
||
//分割
|
||
//方法：先搜索与W最接近的点，然后条件搜索（垂直）与L最接近的点
|
||
double distDeviation = 5.0; //距离搜索的合格门限。小于此距离，认为搜索到的目标为有效
|
||
SVzNLRangeD linePointRange = { 0.3, 0.7 };
|
||
|
||
std::vector< WD_workpieceInfo> allWorkpiece;
|
||
double highest_z = -1;
|
||
for (int objIdx = 0; objIdx < objectSize; objIdx++)
|
||
{
|
||
if (holes[objIdx].radius < 0)
|
||
continue;
|
||
|
||
holes[objIdx].radius = -1;
|
||
SWD_HoleInfo& p0 = holes[objIdx];
|
||
int idx1 = distanceSearchObject(p0.center, holes, workpiecePara.holeDist_W, distDeviation, 0, workpiecePara.H / 2,
|
||
quantiValue, roi2D, linePointRange);
|
||
if (idx1 < 0)
|
||
continue;
|
||
|
||
SVzNLRangeD angleRange = { 80, 100 }; //垂直，5度范围
|
||
SWD_HoleInfo& p1 = holes[idx1];
|
||
//搜索最接近distance且角度为angle的目标, 以角度为优先
|
||
int idx2 = angleConditionDistanceSearch(
|
||
p0.center, p1.center,
|
||
holes,
|
||
workpiecePara.holeDist_L, distDeviation, workpiecePara.H / 2,
|
||
angleRange,
|
||
quantiValue, roi2D, linePointRange);
|
||
if (idx2 < 0)
|
||
continue;
|
||
|
||
SWD_HoleInfo& p2 = holes[idx2];
|
||
//搜索最接近distance且角度为angle的目标, 以角度为优先
|
||
int idx3 = angleConditionDistanceSearch(
|
||
p1.center, p0.center,
|
||
holes,
|
||
workpiecePara.holeDist_L, distDeviation, workpiecePara.H / 2,
|
||
angleRange,
|
||
quantiValue, roi2D, linePointRange);
|
||
if (idx3 < 0)
|
||
continue;
|
||
|
||
SWD_HoleInfo& p3 = holes[idx3];
|
||
|
||
//交叉检查P0P1, P2P3的长度差
|
||
double dist_P0P1 = sqrt(pow(p0.center.x - p1.center.x, 2) + pow(p0.center.y - p1.center.y, 2));
|
||
double dist_P2P3 = sqrt(pow(p2.center.x - p3.center.x, 2) + pow(p2.center.y - p3.center.y, 2));
|
||
double dist_k = dist_P2P3 / dist_P0P1;
|
||
if (dist_k > 2.0)
|
||
continue;
|
||
|
||
p1.radius = -1;
|
||
p2.radius = -1;
|
||
p3.radius = -1;
|
||
|
||
//重新计算Z值。因为沉孔的原因，Z值会不准确。取四条边的中点处的Z值的均值作为整个的Z值
|
||
SVzNL3DPoint center_p0p1 = { (p0.center.x + p1.center.x) / 2,(p0.center.y + p1.center.y) / 2, (p0.center.z + p1.center.z) / 2 };
|
||
SVzNL3DPoint center_p0p2 = { (p0.center.x + p2.center.x) / 2,(p0.center.y + p2.center.y) / 2, (p0.center.z + p2.center.z) / 2 };
|
||
SVzNL3DPoint center_p1p3 = { (p1.center.x + p3.center.x) / 2,(p1.center.y + p3.center.y) / 2, (p1.center.z + p3.center.z) / 2 };
|
||
SVzNL3DPoint center_p2p3 = { (p2.center.x + p3.center.x) / 2,(p2.center.y + p3.center.y) / 2, (p2.center.z + p3.center.z) / 2 };
|
||
double rectR = 5.0;
|
||
double z1 = _getMeanZ(quantiValue, center_p0p1, roi2D, rectR, workpiecePara.H / 2);
|
||
if (z1 < 1e-4)
|
||
z1 = center_p0p1.z;
|
||
double z2 = _getMeanZ(quantiValue, center_p0p2, roi2D, rectR, workpiecePara.H / 2);
|
||
if (z2 < 1e-4)
|
||
z2 = center_p0p2.z;
|
||
double z3 = _getMeanZ(quantiValue, center_p1p3, roi2D, rectR, workpiecePara.H / 2);
|
||
if (z3 < 1e-4)
|
||
z3 = center_p1p3.z;
|
||
double z4 = _getMeanZ(quantiValue, center_p2p3, roi2D, rectR, workpiecePara.H / 2);
|
||
if (z4 < 1e-4)
|
||
z4 = center_p2p3.z;
|
||
|
||
p0.center.z = (z1 + z2) / 2;
|
||
p1.center.z = (z1 + z3) / 2;
|
||
p2.center.z = (z2 + z4) / 2;
|
||
p3.center.z = (z3 + z4) / 2;
|
||
|
||
SVzNL3DPoint centerPoint = { (p0.center.x + p1.center.x + p2.center.x + p3.center.x) / 4,
|
||
(p0.center.y + p1.center.y + p2.center.y + p3.center.y) / 4,
|
||
(z1 + z2 + z3 + z4) / 4 };
|
||
//取工件表面的点
|
||
std::vector<cv::Point3d> Points3ds;
|
||
_getLinePoints(quantiValue, p0.center, p1.center, roi2D, linePointRange, Points3ds);
|
||
int size_1 = (int)Points3ds.size();
|
||
_getLinePoints(quantiValue, p0.center, p2.center, roi2D, linePointRange, Points3ds);
|
||
_getLinePoints(quantiValue, p1.center, p3.center, roi2D, linePointRange, Points3ds);
|
||
_getLinePoints(quantiValue, p2.center, p3.center, roi2D, linePointRange, Points3ds);
|
||
|
||
double minZ = _getMinZ(Points3ds);
|
||
if (minZ > 1e-4)
|
||
{
|
||
if ((highest_z < 0) || (highest_z > minZ))
|
||
highest_z = minZ;
|
||
}
|
||
|
||
//计算工件法向
|
||
SVzNL3DPoint normalDir;
|
||
SVzNL3DPoint vec_norm;
|
||
SVzNL3DPoint y_dir;
|
||
if ((int)(Points3ds.size() > size_1) && (size_1 > 0))
|
||
{
|
||
//计算面参数: z = Ax + By + C
|
||
//res: [0]=A, [1]= B, [2]=-1.0, [3]=C,
|
||
std::vector<double> res;
|
||
vzCaculateLaserPlane(Points3ds, res);
|
||
SVzNL3DPoint vec_1 = { -res[0], -res[1], 1.0 };
|
||
vec_norm = vec3_normalize(vec_1);
|
||
vec_norm = vec3_multiply(vec_norm, 20.0);
|
||
normalDir = { centerPoint.x + vec_norm.x, centerPoint.y + vec_norm.y, centerPoint.z + vec_norm.z };
|
||
|
||
SVzNL3DPoint vector2 = { 0, 0, 1 };
|
||
SSG_planeCalibPara adjustPara = wd_computeRTMatrix(vec_1, vector2);
|
||
SVzNL3DPoint rotate_p0 = _ptRotate(p0.center, adjustPara.planeCalib);
|
||
SVzNL3DPoint rotate_p1 = _ptRotate(p1.center, adjustPara.planeCalib);
|
||
if (rotate_p0.x < rotate_p1.x)
|
||
y_dir = { rotate_p1.x - rotate_p0.x, rotate_p1.y - rotate_p0.y, 0 };
|
||
else
|
||
y_dir = { rotate_p0.x - rotate_p1.x, rotate_p0.y - rotate_p1.y, 0 };
|
||
y_dir = _ptRotate(y_dir, adjustPara.invRMatrix);
|
||
}
|
||
else
|
||
{
|
||
normalDir = { centerPoint.x, centerPoint.y, centerPoint.z + 20 };
|
||
vec_norm = { 0, 0, 20 };
|
||
if (p0.center.x < p1.center.x)
|
||
y_dir = { p1.center.x - p0.center.x, p1.center.y - p0.center.y, 0 };
|
||
else
|
||
y_dir = { p0.center.x - p1.center.x, p0.center.y - p1.center.y, 0 };
|
||
}
|
||
|
||
WD_workpieceInfo a_workpiece;
|
||
a_workpiece.workpieceType = workpiecePara.workpieceType;
|
||
a_workpiece.holes.push_back(p0.center);
|
||
a_workpiece.holes.push_back(p1.center);
|
||
a_workpiece.holes.push_back(p2.center);
|
||
a_workpiece.holes.push_back(p3.center);
|
||
|
||
for (int m = 0; m < 4; m++)
|
||
{
|
||
SVzNL3DPoint a_pt = a_workpiece.holes[m];
|
||
a_pt = { a_pt.x + vec_norm.x, a_pt.y + vec_norm.y, a_pt.z + vec_norm.z };//法向
|
||
a_workpiece.holesDir.push_back(a_pt);
|
||
}
|
||
a_workpiece.center = centerPoint;
|
||
|
||
y_dir = vec3_normalize(y_dir);
|
||
a_workpiece.y_dir = { y_dir.x * 20 + a_workpiece.center.x, y_dir.y * 20 + a_workpiece.center.y, a_workpiece.center.z + y_dir.z * 20 };
|
||
a_workpiece.z_dir = normalDir;
|
||
allWorkpiece.push_back(a_workpiece);
|
||
}
|
||
|
||
int workpieceNum = (int)allWorkpiece.size();
|
||
if (workpieceNum == 0)
|
||
{
|
||
*errCode = SX_ERR_ZERO_OBJECTS;
|
||
return;
|
||
}
|
||
|
||
//寻找最高点
|
||
if (highest_z < 0)
|
||
{
|
||
highest_z = allWorkpiece[0].center.z;
|
||
for (int i = 1; i < workpieceNum; i++)
|
||
{
|
||
if (highest_z > allWorkpiece[i].center.z)
|
||
highest_z = allWorkpiece[i].center.z;
|
||
}
|
||
}
|
||
|
||
//检测上层是否有残留
|
||
double zSliceTh = highest_z - workpiecePara.H/2; //往上工件H的一半
|
||
std::vector<SVzNL3DPosition> topLayerPts;
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
for (int j = 0; j < linePtNum; j++)
|
||
{
|
||
if ((scanLines[line][j].pt3D.z > 1e-4) && (scanLines[line][j].pt3D.z < zSliceTh))
|
||
{
|
||
SVzNL3DPosition a_pt;
|
||
a_pt.nPointIdx = (line << 16) | (j & 0xffff);
|
||
a_pt.pt3D = scanLines[line][j].pt3D;
|
||
topLayerPts.push_back(a_pt);
|
||
}
|
||
}
|
||
}
|
||
//进行聚类，判断聚类大小。大于一定门限判断为有异物
|
||
int topLayerClusterWin = 5;
|
||
double clusterDist = 5.0;
|
||
int distType = 1; //0 - 2d distance; 1- 3d distance
|
||
std::vector<std::vector< SVzNL3DPosition>> objClusters;
|
||
wd_pointClustering_speedUp(
|
||
topLayerPts,
|
||
lineNum, linePtNum, topLayerClusterWin, //搜索窗口
|
||
clusterDist,
|
||
distType, //0 - 2d distance; 1- 3d distance
|
||
objClusters //result
|
||
);
|
||
double vaidObjSize = workpiecePara.xLen * workpiecePara.yLen / 4.0;
|
||
for (int i = 0; i < (int)objClusters.size(); i++)
|
||
{
|
||
SSG_ROIRectD a_roi = _getClusterROI(objClusters[i]);
|
||
double size = (a_roi.right - a_roi.left) * (a_roi.bottom - a_roi.top);
|
||
if (size >= vaidObjSize)
|
||
{
|
||
*errCode = SX_ERR_UNKNOWN_OBJECT;
|
||
return;
|
||
}
|
||
}
|
||
|
||
//排序
|
||
//z方向排序
|
||
std::vector< WD_workpieceInfo> zSortWorkpiece;
|
||
double topLayerTh = highest_z + workpiecePara.H / 2;
|
||
for (int i = 0; i < workpieceNum; i++)
|
||
{
|
||
if (allWorkpiece[i].center.z < topLayerTh)
|
||
zSortWorkpiece.push_back(allWorkpiece[i]);
|
||
}
|
||
//水平方向排序
|
||
while (zSortWorkpiece.size() > 0)
|
||
{
|
||
std::vector< WD_workpieceInfo> firstLine;
|
||
std::vector< WD_workpieceInfo> restWorkpiece;
|
||
_getYTopLine(zSortWorkpiece, firstLine, restWorkpiece, workpiecePara.yLen / 2);
|
||
workpiecePositioning.insert(workpiecePositioning.end(), firstLine.begin(), firstLine.end());
|
||
zSortWorkpiece.clear();
|
||
zSortWorkpiece.insert(zSortWorkpiece.end(), restWorkpiece.begin(), restWorkpiece.end());
|
||
}
|
||
|
||
//旋转回去
|
||
workpieceNum = (int)workpiecePositioning.size();
|
||
for (int i = 0; i < workpieceNum; i++)
|
||
{
|
||
SVzNL3DPoint rpt;
|
||
rpt = _ptRotate(workpiecePositioning[i].center, groundCalibPara.invRMatrix);
|
||
workpiecePositioning[i].center = rpt;
|
||
rpt = _ptRotate(workpiecePositioning[i].y_dir, groundCalibPara.invRMatrix);
|
||
workpiecePositioning[i].y_dir = rpt;
|
||
rpt = _ptRotate(workpiecePositioning[i].z_dir, groundCalibPara.invRMatrix);
|
||
workpiecePositioning[i].z_dir = rpt;
|
||
for (int j = 0, j_max = (int)workpiecePositioning[i].holes.size(); j < j_max; j++)
|
||
{
|
||
rpt = _ptRotate(workpiecePositioning[i].holes[j], groundCalibPara.invRMatrix);
|
||
workpiecePositioning[i].holes[j] = rpt;
|
||
rpt = _ptRotate(workpiecePositioning[i].holesDir[j], groundCalibPara.invRMatrix);
|
||
workpiecePositioning[i].holesDir[j] = rpt;
|
||
}
|
||
|
||
SVzNL3DPoint vector_z = { workpiecePositioning[i].z_dir.x - workpiecePositioning[i].center.x,
|
||
workpiecePositioning[i].z_dir.y - workpiecePositioning[i].center.y,
|
||
workpiecePositioning[i].z_dir.z - workpiecePositioning[i].center.z };
|
||
SVzNL3DPoint vector_y = { workpiecePositioning[i].y_dir.x - workpiecePositioning[i].center.x,
|
||
workpiecePositioning[i].y_dir.y - workpiecePositioning[i].center.y,
|
||
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));
|
||
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));
|
||
vector_y = { vector_y.x / mod_vy, vector_y.y / mod_vy, vector_y.z / mod_vy }; //归一化
|
||
//叉乘出vector_x
|
||
SVzNL3DPoint vector_x;
|
||
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.z = vector_y.x * vector_z.y - vector_z.x * vector_y.y;
|
||
workpiecePositioning[i].x_dir = vector_x;
|
||
workpiecePositioning[i].y_dir = vector_y;
|
||
workpiecePositioning[i].z_dir = vector_z;
|
||
#if 0
|
||
//得到旋转矩阵
|
||
double R[3][3];
|
||
R[0][0] = vector_x.x;
|
||
R[1][0] = vector_x.y;
|
||
R[2][0] = vector_x.z;
|
||
|
||
R[0][1] = vector_y.x;
|
||
R[1][1] = vector_y.y;
|
||
R[2][1] = vector_y.z;
|
||
|
||
R[0][2] = vector_z.x;
|
||
R[1][2] = vector_z.y;
|
||
R[2][2] = vector_z.z;
|
||
SSG_EulerAngles eulerAngle = rotationMatrixToEulerZYX(R);
|
||
workpiecePositioning[i].workpiecePose = eulerAngle;
|
||
#endif
|
||
}
|
||
return;
|
||
|
||
}
|
||
|
||
typedef struct
|
||
{
|
||
int flag;
|
||
SWDIndexing3DPoint zMaxPos;
|
||
SVzNLRect peakROI;
|
||
double pkValue;
|
||
}_zMaxInfo;
|
||
|
||
bool _getZMaxPeakROI(SVzNL2DPoint seedPos,
|
||
std::vector<std::vector<SSG_pntDirAngle>>& pntDirAngles_v,
|
||
std::vector<std::vector<SSG_pntDirAngle>>& pntDirAngles_h,
|
||
double pntCornenrMax,
|
||
double minZHeight,
|
||
SVzNLRect& roi
|
||
)
|
||
{
|
||
int lineNum = (int)pntDirAngles_v.size();
|
||
int ptNum = (int)pntDirAngles_h.size();
|
||
|
||
double seed_z = pntDirAngles_v[seedPos.x][seedPos.y].curr_z;
|
||
//向左搜索
|
||
int left = -1;
|
||
for (int line = seedPos.x; line >= 0; line--)
|
||
{
|
||
double z_diff = seed_z - pntDirAngles_v[line][seedPos.y].curr_z;
|
||
if ((pntDirAngles_v[line][seedPos.y].type >= 0) && (abs(pntDirAngles_v[line][seedPos.y].corner) < pntCornenrMax)
|
||
&& (z_diff > minZHeight))
|
||
{
|
||
left = line;
|
||
break;
|
||
}
|
||
}
|
||
//向右搜索
|
||
int right = -1;
|
||
for (int line = seedPos.x; line < lineNum; line++)
|
||
{
|
||
double z_diff = seed_z - pntDirAngles_v[line][seedPos.y].curr_z;
|
||
if ((pntDirAngles_v[line][seedPos.y].type >= 0) && (abs(pntDirAngles_v[line][seedPos.y].corner) < pntCornenrMax)
|
||
&& (z_diff > minZHeight))
|
||
{
|
||
right = line;
|
||
break;
|
||
}
|
||
}
|
||
//向上搜索
|
||
int top = -1;
|
||
for (int ptIdx = seedPos.y; ptIdx >= 0; ptIdx--)
|
||
{
|
||
double z_diff = seed_z - pntDirAngles_h[ptIdx][seedPos.x].curr_z;
|
||
if ((pntDirAngles_h[ptIdx][seedPos.x].type >= 0) && (abs(pntDirAngles_h[ptIdx][seedPos.x].corner) < pntCornenrMax)
|
||
&& (z_diff > minZHeight))
|
||
{
|
||
top = ptIdx;
|
||
break;
|
||
}
|
||
}
|
||
//向下搜索
|
||
int bottom = -1;
|
||
for (int ptIdx = seedPos.y; ptIdx < ptNum; ptIdx++)
|
||
{
|
||
double z_diff = seed_z - pntDirAngles_h[ptIdx][seedPos.x].curr_z;
|
||
if ((pntDirAngles_h[ptIdx][seedPos.x].type >= 0) && (abs(pntDirAngles_h[ptIdx][seedPos.x].corner) < pntCornenrMax)
|
||
&& (z_diff > minZHeight))
|
||
{
|
||
bottom = ptIdx;
|
||
break;
|
||
}
|
||
}
|
||
if ((left < 0) || (right < 0) || (top < 0) || (bottom < 0))
|
||
return false;
|
||
roi.left = left;
|
||
roi.right = right;
|
||
roi.top = top;
|
||
roi.bottom = bottom;
|
||
return true;
|
||
}
|
||
|
||
SVzNLRect _mergeROI(SVzNLRect roi_1, SVzNLRect roi_2)
|
||
{
|
||
SVzNLRect roi;
|
||
roi.left = roi_1.left < roi_2.left ? roi_1.left : roi_2.left;
|
||
roi.right = roi_1.right > roi_2.right ? roi_1.right : roi_2.right;
|
||
roi.top = roi_1.top < roi_2.top ? roi_1.top : roi_2.top;
|
||
roi.bottom = roi_1.bottom > roi_2.bottom ? roi_1.bottom : roi_2.bottom;
|
||
return roi;
|
||
}
|
||
|
||
//单个孔或凹坑定位-华航孔定位
|
||
void wd_HolePositioning(
|
||
std::vector< std::vector<SVzNL3DPosition>>& scanLinesInput,
|
||
const SSG_lineSegParam lineSegPara,
|
||
const SSG_cornerParam cornerParam,
|
||
const SSG_outlierFilterParam filterParam,
|
||
const SSG_treeGrowParam growParam,
|
||
std::vector< WD_HolePositionInfo>& holePositioning,
|
||
int* errCode)
|
||
{
|
||
*errCode = 0;
|
||
|
||
int lineNum = (int)scanLinesInput.size();
|
||
std::vector< std::vector<SVzNL3DPosition>> scanLines;
|
||
scanLines.resize(lineNum);
|
||
int linePtNum = (int)scanLinesInput[0].size();
|
||
bool isGridData = true;
|
||
for (int i = 0; i < lineNum; i++)
|
||
{
|
||
if (linePtNum != (int)scanLinesInput[i].size())
|
||
isGridData = false;
|
||
|
||
scanLines[i].resize(scanLinesInput[i].size());
|
||
std::copy(scanLinesInput[i].begin(), scanLinesInput[i].end(), scanLines[i].begin()); // 使用std::copy算法
|
||
|
||
for (int j = 0; j < (int)scanLinesInput[i].size(); j++)
|
||
scanLinesInput[i][j].nPointIdx = 0; //清零，用于debug时记录信息
|
||
}
|
||
if (false == isGridData)//数据不是网格格式
|
||
{
|
||
*errCode = SG_ERR_NOT_GRID_FORMAT;
|
||
return;
|
||
}
|
||
|
||
//添加3x3平滑
|
||
scanLinesSmooting3x3(scanLinesInput, scanLines);
|
||
|
||
//内部参数
|
||
double zPeakScale = 10.0; //计算ZPeak时的尺度
|
||
double minZPeakHeight = 2.0; //最小的凹坑深度
|
||
double planeInlierDistTh = 1.5; //平面点距离平面的距离。超出此距离被判别为离群点
|
||
double flagCornerMax = 3.0; //直线上点的拐角最大值
|
||
double maxHoleDiameter = 30.0;//最大的孔直径
|
||
double valieCommonNumRatio = 0.125; //1/8
|
||
//计算dirAngle
|
||
std::vector<std::vector<SSG_pntDirAngle>> pntDirAngles_v;
|
||
std::vector<std::vector<SSG_basicFeature1D>> zMaxPeaks_v;
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
if (line == 1062)
|
||
int kkk = 1;
|
||
|
||
std::vector<SVzNL3DPosition>& lineData = scanLines[line];
|
||
//滤波，滤除异常点
|
||
//sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam);
|
||
|
||
std::vector< SSG_pntDirAngle> line_ptDirAngles;
|
||
wd_computeDirAngle_wholeLine(lineData, cornerParam, line_ptDirAngles);
|
||
pntDirAngles_v.push_back(line_ptDirAngles);
|
||
|
||
std::vector< SSG_basicFeature1D> localZMax;
|
||
std::vector< SSG_basicFeature1D> localZMin;
|
||
sg_getLineLocalPeaks_2( lineData, line, zPeakScale, localZMax, localZMin);
|
||
zMaxPeaks_v.push_back(localZMax);
|
||
}
|
||
|
||
//生成水平扫描
|
||
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特征提取
|
||
//创建水平ZMax的Mask
|
||
std::vector<std::vector<int>> zMaxMask_h;
|
||
zMaxMask_h.resize(lineNum);
|
||
for (int i = 0; i < lineNum; i++)
|
||
{
|
||
zMaxMask_h[i].resize(linePtNum);
|
||
std::fill(zMaxMask_h[i].begin(), zMaxMask_h[i].end(), 0);
|
||
}
|
||
|
||
std::vector<std::vector<SSG_pntDirAngle>> pntDirAngles_h;
|
||
for (int line = 0; line < linePtNum; line++)
|
||
{
|
||
if (line == 234)
|
||
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_pntDirAngle> line_ptDirAngles;
|
||
wd_computeDirAngle_wholeLine(lineData, cornerParam, line_ptDirAngles);
|
||
pntDirAngles_h.push_back(line_ptDirAngles);
|
||
|
||
std::vector< SSG_basicFeature1D> localZMax;
|
||
std::vector< SSG_basicFeature1D> localZMin;
|
||
sg_getLineLocalPeaks_2(lineData, line, zPeakScale, localZMax, localZMin);
|
||
for (int j = 0; j < (int)localZMax.size(); j++)
|
||
{
|
||
int idx_line = localZMax[j].jumpPos2D.y;
|
||
int idx_pt = localZMax[j].jumpPos2D.x;
|
||
zMaxMask_h[idx_line][idx_pt] = 1;
|
||
}
|
||
}
|
||
|
||
//判断真正的ZMax并记录
|
||
//（1）真正的ZMax一定是同时为X方向的ZMax和Y方向的ZMax.(2)满足一定的Z差值
|
||
std::vector<_zMaxInfo> objPeaks;
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
if (line == 144)
|
||
int kkk = 1;
|
||
std::vector<SSG_basicFeature1D>& a_lineZMax = zMaxPeaks_v[line];
|
||
for (int j = 0; j < (int)a_lineZMax.size(); j++)
|
||
{
|
||
int idx_line = a_lineZMax[j].jumpPos2D.x;
|
||
int idx_pt = a_lineZMax[j].jumpPos2D.y;
|
||
if (zMaxMask_h[idx_line][idx_pt] > 0) //在X和Y方向均为极大值
|
||
{
|
||
//判断ROI
|
||
SVzNLRect a_roi;
|
||
bool validPk = _getZMaxPeakROI(a_lineZMax[j].jumpPos2D, pntDirAngles_v, pntDirAngles_h, flagCornerMax, minZPeakHeight, a_roi);
|
||
if (true == validPk)
|
||
{
|
||
double meanZ = scanLines[a_roi.left][idx_pt].pt3D.z;
|
||
double minZ = scanLines[a_roi.left][idx_pt].pt3D.z;
|
||
double maxZ = scanLines[a_roi.left][idx_pt].pt3D.z;
|
||
meanZ += scanLines[a_roi.right][idx_pt].pt3D.z;
|
||
minZ = minZ < scanLines[a_roi.right][idx_pt].pt3D.z ? minZ : scanLines[a_roi.right][idx_pt].pt3D.z;
|
||
maxZ = maxZ > scanLines[a_roi.right][idx_pt].pt3D.z ? maxZ : scanLines[a_roi.right][idx_pt].pt3D.z;
|
||
meanZ += scanLines[idx_line][a_roi.top].pt3D.z;
|
||
minZ = minZ < scanLines[idx_line][a_roi.top].pt3D.z ? minZ : scanLines[idx_line][a_roi.top].pt3D.z;
|
||
maxZ = maxZ > scanLines[idx_line][a_roi.top].pt3D.z ? maxZ : scanLines[idx_line][a_roi.top].pt3D.z;
|
||
meanZ += scanLines[idx_line][a_roi.bottom].pt3D.z;
|
||
minZ = minZ < scanLines[idx_line][a_roi.bottom].pt3D.z ? minZ : scanLines[idx_line][a_roi.bottom].pt3D.z;
|
||
maxZ = maxZ > scanLines[idx_line][a_roi.bottom].pt3D.z ? maxZ : scanLines[idx_line][a_roi.bottom].pt3D.z;
|
||
double zMaxMin = maxZ - minZ;
|
||
meanZ = meanZ / 4.0;
|
||
double z_diff = a_lineZMax[j].jumpPos.z - meanZ;
|
||
|
||
double xLen = abs(scanLines[a_roi.right][idx_pt].pt3D.x - scanLines[a_roi.left][idx_pt].pt3D.x);
|
||
double yLen = abs(scanLines[idx_line][a_roi.bottom].pt3D.y - scanLines[idx_line][a_roi.top].pt3D.y);
|
||
if ( (z_diff > minZPeakHeight) && (xLen < maxHoleDiameter) && (yLen < maxHoleDiameter) && (zMaxMin < planeInlierDistTh*2))
|
||
{
|
||
_zMaxInfo a_info;
|
||
a_info.flag = 0;
|
||
a_info.peakROI = a_roi;
|
||
a_info.zMaxPos.lineIdx = a_lineZMax[j].jumpPos2D.x;
|
||
a_info.zMaxPos.ptIdx = a_lineZMax[j].jumpPos2D.y;
|
||
a_info.zMaxPos.point = { a_lineZMax[j].jumpPos.x, a_lineZMax[j].jumpPos.y, a_lineZMax[j].jumpPos.z };
|
||
a_info.pkValue = a_lineZMax[j].jumpPos.z;
|
||
objPeaks.push_back(a_info);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
int pkSize = (int)objPeaks.size();
|
||
for (int i = 0; i < pkSize; i++)
|
||
{
|
||
_zMaxInfo a_info = objPeaks[i];
|
||
if (a_info.flag < 0)
|
||
continue;
|
||
|
||
for (int j = i + 1; j < pkSize; j++)
|
||
{
|
||
if (objPeaks[j].flag < 0)
|
||
continue;
|
||
|
||
if ((a_info.peakROI.right >= objPeaks[j].peakROI.left) && (objPeaks[j].peakROI.right >= a_info.peakROI.left) &&
|
||
(a_info.peakROI.bottom >= objPeaks[j].peakROI.top) && (objPeaks[j].peakROI.bottom >= a_info.peakROI.top)) //重叠
|
||
{
|
||
a_info.peakROI = _mergeROI(a_info.peakROI, objPeaks[j].peakROI);
|
||
if (a_info.pkValue < objPeaks[j].pkValue)
|
||
{
|
||
a_info.pkValue = objPeaks[j].pkValue;
|
||
a_info.zMaxPos = objPeaks[j].zMaxPos;
|
||
}
|
||
objPeaks[j].flag = -1;
|
||
}
|
||
}
|
||
objPeaks[i] = a_info;
|
||
}
|
||
//逐个目标提取
|
||
int contourWin = 4; //取周围3行和3列的点
|
||
for (int peakIdx = 0; peakIdx < pkSize; peakIdx++)
|
||
{
|
||
if (objPeaks[peakIdx].flag < 0)
|
||
continue;
|
||
|
||
SVzNLRect& a_roi = objPeaks[peakIdx].peakROI;
|
||
//提取孔周围点
|
||
SVzNLRect extend_roi = { a_roi.left - contourWin, a_roi.right + contourWin, a_roi.top - contourWin , a_roi.bottom + contourWin };
|
||
if (extend_roi.left < 0) extend_roi.left = 0;
|
||
if (extend_roi.right >= lineNum) extend_roi.right = lineNum - 1;
|
||
if (extend_roi.top < 0) extend_roi.top = 0;
|
||
if (extend_roi.bottom >= linePtNum) extend_roi.bottom = linePtNum - 1;
|
||
|
||
std::vector<cv::Point3d> Points3ds;
|
||
//左
|
||
for (int line = extend_roi.left; line <= a_roi.left-1; line++)
|
||
{
|
||
for (int j = a_roi.top; j <= a_roi.bottom; j++)
|
||
{
|
||
if (scanLines[line][j].pt3D.z > 1e-4)
|
||
{
|
||
cv::Point3d a_pt = cv::Point3d(scanLines[line][j].pt3D.x , scanLines[line][j].pt3D.y, scanLines[line][j].pt3D.z);
|
||
Points3ds.push_back(a_pt);
|
||
}
|
||
}
|
||
}
|
||
//右
|
||
for (int line = a_roi.right+1; line <=extend_roi.right; line++)
|
||
{
|
||
for (int j = a_roi.top; j <= a_roi.bottom; j++)
|
||
{
|
||
if (scanLines[line][j].pt3D.z > 1e-4)
|
||
{
|
||
cv::Point3d a_pt = cv::Point3d(scanLines[line][j].pt3D.x, scanLines[line][j].pt3D.y, scanLines[line][j].pt3D.z);
|
||
Points3ds.push_back(a_pt);
|
||
}
|
||
}
|
||
}
|
||
//上
|
||
for (int ptIdx = extend_roi.top; ptIdx <= a_roi.top-1; ptIdx++)
|
||
{
|
||
for (int j = a_roi.left; j <= a_roi.right; j++)
|
||
{
|
||
if (scanLines[j][ptIdx].pt3D.z > 1e-4)
|
||
{
|
||
cv::Point3d a_pt = cv::Point3d(scanLines[j][ptIdx].pt3D.x, scanLines[j][ptIdx].pt3D.y, scanLines[j][ptIdx].pt3D.z);
|
||
Points3ds.push_back(a_pt);
|
||
}
|
||
}
|
||
}
|
||
//下
|
||
for (int ptIdx = a_roi.bottom+1; ptIdx < extend_roi.bottom; ptIdx++)
|
||
{
|
||
for (int j = a_roi.left; j <= a_roi.right; j++)
|
||
{
|
||
if (scanLines[j][ptIdx].pt3D.z > 1e-4)
|
||
{
|
||
cv::Point3d a_pt = cv::Point3d(scanLines[j][ptIdx].pt3D.x, scanLines[j][ptIdx].pt3D.y, scanLines[j][ptIdx].pt3D.z);
|
||
Points3ds.push_back(a_pt);
|
||
}
|
||
}
|
||
}
|
||
//计算面参数: z = Ax + By + C , res: [0]=A, [1]= B, [2]=-1.0, [3]=C,
|
||
std::vector<double> res;
|
||
#if 0
|
||
vzCaculateLaserPlane(Points3ds, res);
|
||
#else
|
||
#if 0
|
||
double delta = 1.0;
|
||
int maxIter = 20;
|
||
Plane robustPlane = robustFitPlane(
|
||
Points3ds, TUKEY,
|
||
delta , // 阈值：>此值视为离群点（mm）
|
||
maxIter // 迭代次数
|
||
);
|
||
#else
|
||
float dist_thresh = 0.5f;
|
||
int max_iter = 1000;
|
||
int stop_no_improve = 150;
|
||
std::vector<cv::Point3d> out_inliers;
|
||
Plane robustPlane = ransacFitPlane(
|
||
Points3ds,
|
||
out_inliers,
|
||
dist_thresh, // 内点距离阈值
|
||
max_iter, // 最大迭代
|
||
stop_no_improve // 连续多少次无提升就提前退出
|
||
);
|
||
#endif
|
||
res.resize(4);
|
||
if (robustPlane.C > 0)
|
||
{
|
||
res[0] = -robustPlane.A;
|
||
res[1] = -robustPlane.B;
|
||
res[2] = -robustPlane.C;
|
||
res[3] = -robustPlane.D;
|
||
}
|
||
else
|
||
{
|
||
res[0] = robustPlane.A;
|
||
res[1] = robustPlane.B;
|
||
res[2] = robustPlane.C;
|
||
res[3] = robustPlane.D;
|
||
}
|
||
#endif
|
||
double normValue = sqrt(pow(res[0], 2) + pow(res[1], 2) + pow(res[2],2));
|
||
double norm_A = res[0] / normValue;
|
||
double norm_B = res[1] / normValue;
|
||
double norm_C = res[2] / normValue;
|
||
double norm_D = res[3] / normValue;
|
||
//生成ROI scanLines
|
||
int roi_lineNum = extend_roi.right - extend_roi.left + 1;
|
||
int roi_ptNum = extend_roi.bottom - extend_roi.top + 1;
|
||
std::vector< std::vector<SVzNL3DPosition>> roi_scanLines;
|
||
roi_scanLines.resize(roi_lineNum);
|
||
for (int j = 0; j < roi_lineNum; j++)
|
||
roi_scanLines[j].resize(roi_ptNum);
|
||
|
||
//生成面上点
|
||
for (int line = extend_roi.left; line <= extend_roi.right; line++)
|
||
{
|
||
int roi_line = line - extend_roi.left;
|
||
for (int ptIdx = extend_roi.top; ptIdx <= extend_roi.bottom; ptIdx++)
|
||
{
|
||
int roi_ptIdx = ptIdx - extend_roi.top;
|
||
roi_scanLines[roi_line][roi_ptIdx] = scanLines[line][ptIdx];
|
||
//最外圈不进行处理
|
||
if ((line != extend_roi.left) && (line != extend_roi.right) && (ptIdx != extend_roi.top) && (ptIdx != extend_roi.bottom))
|
||
{
|
||
if (scanLines[line][ptIdx].pt3D.z > 1e-4)
|
||
{
|
||
SVzNL3DPoint& a_pt3D = scanLines[line][ptIdx].pt3D;
|
||
double dist = abs(norm_A * a_pt3D.x + norm_B * a_pt3D.y + norm_C * a_pt3D.z + norm_D);
|
||
if (dist > planeInlierDistTh)
|
||
roi_scanLines[roi_line][roi_ptIdx].pt3D = { 0, 0, 0 };
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
std::vector<SWD_segFeatureTree> segTrees_v;
|
||
std::vector<SWD_segFeatureTree> segTrees_h;
|
||
std::vector<SSG_intPair> validObjects;
|
||
WD_getHoleInfo(roi_scanLines, lineSegPara, filterParam, growParam, valieCommonNumRatio, segTrees_v, segTrees_h, validObjects);
|
||
if (validObjects.size() > 0)
|
||
{
|
||
SSG_intPair a_hvPair = validObjects[0];
|
||
if (validObjects.size() > 1)
|
||
{
|
||
for (int j = 1; j < (int)validObjects.size(); j++)
|
||
{
|
||
if (a_hvPair.idx < validObjects[j].idx)
|
||
a_hvPair = validObjects[j];
|
||
}
|
||
}
|
||
//生成Contour
|
||
std::vector<SVzNL3DPoint> cluster_pointArray;
|
||
int vTreeIdx = a_hvPair.data_0;
|
||
int hTreeIdx = a_hvPair.data_1;
|
||
for (int m = 0; m < (int)segTrees_v[vTreeIdx].treeNodes.size(); m++)
|
||
{
|
||
SWD_segFeature& a_seg = segTrees_v[vTreeIdx].treeNodes[m];
|
||
if (roi_scanLines[a_seg.lineIdx][a_seg.endPtIdx].nPointIdx == 0)
|
||
{
|
||
roi_scanLines[a_seg.lineIdx][a_seg.endPtIdx].nPointIdx = vTreeIdx + 1; // 0x01;
|
||
SVzNL3DPoint a_pos = roi_scanLines[a_seg.lineIdx][a_seg.endPtIdx].pt3D;
|
||
cluster_pointArray.push_back(a_pos);
|
||
}
|
||
if (roi_scanLines[a_seg.lineIdx][a_seg.startPtIdx].nPointIdx == 0)
|
||
{
|
||
roi_scanLines[a_seg.lineIdx][a_seg.startPtIdx].nPointIdx = vTreeIdx + 1; // 0x01;
|
||
SVzNL3DPoint a_pos = roi_scanLines[a_seg.lineIdx][a_seg.startPtIdx].pt3D;
|
||
cluster_pointArray.push_back(a_pos);
|
||
}
|
||
}
|
||
for (int m = 0; m < (int)segTrees_h[hTreeIdx].treeNodes.size(); m++)
|
||
{
|
||
SWD_segFeature& a_seg = segTrees_h[hTreeIdx].treeNodes[m];
|
||
if (roi_scanLines[a_seg.startPtIdx][a_seg.lineIdx].nPointIdx == 0)
|
||
{
|
||
roi_scanLines[a_seg.startPtIdx][a_seg.lineIdx].nPointIdx = vTreeIdx + 1; // 0x02;
|
||
SVzNL3DPoint a_pos = roi_scanLines[a_seg.startPtIdx][a_seg.lineIdx].pt3D;
|
||
cluster_pointArray.push_back(a_pos);
|
||
}
|
||
if (roi_scanLines[a_seg.endPtIdx][a_seg.lineIdx].nPointIdx == 0)
|
||
{
|
||
roi_scanLines[a_seg.endPtIdx][a_seg.lineIdx].nPointIdx = vTreeIdx + 1; // 0x02;
|
||
SVzNL3DPoint a_pos = roi_scanLines[a_seg.endPtIdx][a_seg.lineIdx].pt3D;
|
||
cluster_pointArray.push_back(a_pos);
|
||
}
|
||
}
|
||
//计算重心
|
||
#if 0
|
||
SVzNL3DPoint center;
|
||
double radius;
|
||
double err = fitCircleByLeastSquare(cluster_pointArray, center, radius);
|
||
#endif
|
||
if (cluster_pointArray.size() > 0)
|
||
{
|
||
SVzNL3DPoint center = { 0, 0, 0 };
|
||
int ptSize = (int)cluster_pointArray.size();
|
||
for (int m = 0; m < ptSize; m++)
|
||
{
|
||
center.x += cluster_pointArray[m].x;
|
||
center.y += cluster_pointArray[m].y;
|
||
center.z += cluster_pointArray[m].z;
|
||
}
|
||
center.x = center.x / ptSize;
|
||
center.y = center.y / ptSize;
|
||
center.z = center.z / ptSize;
|
||
//求在平面上的垂足
|
||
double t = -(center.x * norm_A + center.y * norm_B + center.z * norm_C + norm_D);
|
||
SVzNL3DPoint realCenter;
|
||
realCenter.x = center.x + t * norm_A;
|
||
realCenter.y = center.y + t * norm_B;
|
||
realCenter.z = center.z + t * norm_C;
|
||
WD_HolePositionInfo a_hole;
|
||
a_hole.center = realCenter;
|
||
a_hole.normDir = { norm_A, norm_B, norm_C };
|
||
holePositioning.push_back(a_hole);
|
||
}
|
||
}
|
||
|
||
}
|
||
return;
|
||
}
|
||
|
||
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;
|
||
}
|
||
|
||
double _getListMeanZ(std::vector< SVzNL3DPosition>& listData, SVzNLRangeD& zRange)
|
||
{
|
||
if (listData.size() == 0)
|
||
return 0;
|
||
double meanZ = 0;
|
||
zRange.max = -1;
|
||
zRange.min = 0;
|
||
for (int i = 0; i < (int)listData.size(); i++)
|
||
{
|
||
meanZ += listData[i].pt3D.z;
|
||
if (zRange.max < 0)
|
||
{
|
||
zRange.max = listData[i].pt3D.z;
|
||
zRange.min = listData[i].pt3D.z;
|
||
}
|
||
else
|
||
{
|
||
zRange.max = zRange.max < listData[i].pt3D.z ? listData[i].pt3D.z : zRange.max;
|
||
zRange.min = zRange.min > listData[i].pt3D.z ? listData[i].pt3D.z : zRange.min;
|
||
}
|
||
}
|
||
meanZ = meanZ / (double)listData.size();
|
||
return meanZ;
|
||
}
|
||
|
||
//砂轮盘中心孔定位
|
||
WD_HolePositionInfo sx_getDiscHolePose(
|
||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||
const SSG_cornerParam cornerPara,
|
||
int* errCode)
|
||
{
|
||
*errCode = 0;
|
||
WD_HolePositionInfo resultPose;
|
||
resultPose.center = { 0, 0, 0 };
|
||
resultPose.normDir = { 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;
|
||
}
|
||
|
||
//算法流程：
|
||
//1、检查垂直方向数据并去除
|
||
//2、聚类
|
||
//3、保留最前面目标
|
||
//4、提取孔
|
||
//5、拟合
|
||
//6、计算中间坐标
|
||
//内部参数
|
||
SSG_cornerParam removeVertialPara = cornerPara;
|
||
removeVertialPara.scale = 3.0;
|
||
removeVertialPara.cornerTh = 60;
|
||
|
||
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<std::vector<int>> zVertivalFlags;
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
if (line == 700)
|
||
int kkk = 1;
|
||
std::vector<int> line_verticalFlags;
|
||
wd_getXYVertialFeature_dirAngleMethod(
|
||
scanLines[line],
|
||
line,
|
||
removeVertialPara,
|
||
line_verticalFlags
|
||
);
|
||
zVertivalFlags.push_back(line_verticalFlags);
|
||
|
||
for (int i = 0; i < (int)line_verticalFlags.size(); i++)
|
||
{
|
||
if (line_verticalFlags[i] > 0)
|
||
flags[line][i] = 1;
|
||
}
|
||
}
|
||
|
||
std::vector<std::vector<int>> zVertivalFlags_h;
|
||
for (int line = 0; line < linePtNum; line++)
|
||
{
|
||
if (line == 1177)
|
||
int kkk = 1;
|
||
std::vector<int> line_verticalFlags;
|
||
wd_getXYVertialFeature_dirAngleMethod(
|
||
scanLines_h[line],
|
||
line,
|
||
removeVertialPara,
|
||
line_verticalFlags
|
||
);
|
||
zVertivalFlags_h.push_back(line_verticalFlags);
|
||
|
||
for (int i = 0; i < (int)line_verticalFlags.size(); i++)
|
||
{
|
||
if (line_verticalFlags[i] > 0)
|
||
flags[i][line] = 1;
|
||
}
|
||
}
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
for (int j = 0; j < linePtNum; j++)
|
||
{
|
||
if (flags[line][j] > 0)
|
||
{
|
||
scanLines[line][j].pt3D.z = 0;
|
||
scanLines_h[j][line].pt3D.z = 0;
|
||
}
|
||
}
|
||
}
|
||
//迭代一次
|
||
SSG_lineSegParam lineSegPara;
|
||
lineSegPara.distScale = 5.0;
|
||
lineSegPara.segGapTh_y = 5.0;
|
||
lineSegPara.segGapTh_z = 5.0;
|
||
const int minSegLen = 5;
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
std::vector<SSG_RUN> segs;
|
||
wd_getLineDataIntervals(
|
||
scanLines[line],
|
||
lineSegPara,
|
||
segs);
|
||
for (int i = 0; i < (int)segs.size(); i++)
|
||
{
|
||
if (segs[i].len <= minSegLen)
|
||
{
|
||
int idx0 = segs[i].start;
|
||
for (int j = 0; j < segs[i].len; j++)
|
||
flags[line][idx0 + j] = 1;
|
||
}
|
||
}
|
||
}
|
||
for (int line = 0; line < linePtNum; line++)
|
||
{
|
||
std::vector<SSG_RUN> segs;
|
||
wd_getLineDataIntervals(
|
||
scanLines_h[line],
|
||
lineSegPara,
|
||
segs);
|
||
for (int i = 0; i < (int)segs.size(); i++)
|
||
{
|
||
if (segs[i].len <= minSegLen)
|
||
{
|
||
int idx0 = segs[i].start;
|
||
for (int j = 0; j < segs[i].len; j++)
|
||
flags[idx0 + j][line] = 1;
|
||
}
|
||
}
|
||
}
|
||
|
||
//标注
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
for (int j = 0; j < linePtNum; j++)
|
||
scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
|
||
}
|
||
//将垂直线段去除
|
||
std::vector< SVzNL3DPosition> validPoints;
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
for (int j = 0; j < linePtNum; j++)
|
||
{
|
||
if (flags[line][j] > 0)
|
||
scanLines[line][j].pt3D.z = 0;
|
||
|
||
if (scanLines[line][j].pt3D.z > 1e-4)
|
||
{
|
||
SVzNL3DPosition a_vldPt;
|
||
a_vldPt.pt3D = scanLines[line][j].pt3D;
|
||
a_vldPt.nPointIdx = (line << 16) | (j & 0xffff);
|
||
validPoints.push_back(a_vldPt);
|
||
}
|
||
}
|
||
}
|
||
|
||
//聚类
|
||
//内部参数
|
||
//double minObjSize_w = 150;
|
||
//double minObjSize_h = 150;
|
||
|
||
int clusterCheckWin = 5;
|
||
double clusterDist = 1.5;
|
||
int distType = 1; //0 - 2d distance; 1- 3d distance
|
||
std::vector<std::vector< SVzNL3DPosition>> objClusters; //result
|
||
wd_pointClustering_speedUp(
|
||
validPoints,
|
||
lineNum, linePtNum, clusterCheckWin, //搜索窗口
|
||
clusterDist,
|
||
distType,
|
||
objClusters //result
|
||
);
|
||
|
||
//保留最前面的目标
|
||
const double topPlateMinW = 150.0;
|
||
const double topPlateMinH = 150.0;
|
||
const double topPlateMaxZRange = 100.0;
|
||
std::vector<double> objMeanZ;
|
||
std::vector<SVzNLRangeD> objZRange;
|
||
objMeanZ.resize(objClusters.size());
|
||
objZRange.resize(objClusters.size());
|
||
for (int i = 0; i < (int)objClusters.size(); i++)
|
||
{
|
||
SSG_ROIRectD a_roi = _getListROI(objClusters[i]);
|
||
double w = a_roi.right - a_roi.left;
|
||
double h = a_roi.bottom - a_roi.top;
|
||
if ((w > topPlateMinW) && (h > topPlateMinH))
|
||
{
|
||
SVzNLRangeD zRange;
|
||
double meanZ = _getListMeanZ(objClusters[i], zRange);
|
||
objMeanZ[i] = meanZ;
|
||
objZRange[i] = zRange;
|
||
}
|
||
else
|
||
{
|
||
objMeanZ[i] = 0;
|
||
objZRange[i].max = -1.0;
|
||
objZRange[i].min = 0.0;
|
||
}
|
||
}
|
||
|
||
//选出z最小的目标作为顶部轮廓
|
||
int objIdx = -1;
|
||
double minMeanZ = DBL_MAX;
|
||
for (int i = 0; i < (int)objClusters.size(); i++)
|
||
{
|
||
if ((objMeanZ[i] > 1e-4) && (objZRange[i].max > 0))
|
||
{
|
||
double range = objZRange[i].max - objZRange[i].min;
|
||
if (range < topPlateMaxZRange)
|
||
{
|
||
if (minMeanZ > objMeanZ[i])
|
||
{
|
||
minMeanZ = objMeanZ[i];
|
||
objIdx = i;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
if (objIdx < 0)
|
||
{
|
||
*errCode = SG_ERR_ZERO_OBJECTS;
|
||
return resultPose;
|
||
}
|
||
|
||
std::vector< SVzNL3DPosition>& topCluster = objClusters[objIdx];
|
||
//标注
|
||
//重新将flags设置为目标的mask
|
||
for (int i = 0; i < lineNum; i++)
|
||
std::fill(flags[i].begin(), flags[i].end(), -1);
|
||
for (int i = 0; i < (int)topCluster.size(); i++)
|
||
{
|
||
int line = topCluster[i].nPointIdx >> 16;
|
||
int ptIdx = topCluster[i].nPointIdx & 0x0000FFFF;
|
||
scanLines[line][ptIdx].nPointIdx = 2;
|
||
flags[line][ptIdx] = i; //indexing
|
||
}
|
||
|
||
//使用PCA方法计算法向量
|
||
SVzNL3DPoint vec_normal, vec_centroid;
|
||
|
||
computePlaneNormalByPCA(
|
||
topCluster,
|
||
vec_normal,
|
||
vec_centroid);
|
||
|
||
if (vec_normal.z < 0)
|
||
vec_normal = { -vec_normal.x, -vec_normal.y, -vec_normal.z };
|
||
resultPose.center = vec_centroid;
|
||
resultPose.normDir = vec_normal;
|
||
return resultPose;
|
||
}
|
||
|
||
//砂轮盘架子中心定位
|
||
SSG_pointPose sx_getDiscRackCenterPosition(
|
||
std::vector< std::vector<SVzNL3DPosition>>& src_scanLines,
|
||
const SSG_cornerParam cornerPara,
|
||
int* errCode)
|
||
{
|
||
*errCode = 0;
|
||
SSG_pointPose resultPose;
|
||
resultPose.point = { 0, 0, 0 };
|
||
resultPose.pose_x = { 0, 0, 0 };
|
||
resultPose.pose_y = { 0, 0, 0 };
|
||
resultPose.pose_z = { 0, 0, 0 };
|
||
|
||
int lineNum = (int)src_scanLines.size();
|
||
if (lineNum == 0)
|
||
{
|
||
*errCode = SG_ERR_3D_DATA_NULL;
|
||
return resultPose;
|
||
}
|
||
|
||
int linePtNum = (int)src_scanLines[0].size();
|
||
|
||
//判断数据格式是否为grid。算法只能处理grid数据格式
|
||
bool isGridData = true;
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
if (linePtNum != (int)src_scanLines[line].size())
|
||
{
|
||
isGridData = false;
|
||
break;
|
||
}
|
||
}
|
||
if (false == isGridData)//数据不是网格格式
|
||
{
|
||
*errCode = SG_ERR_NOT_GRID_FORMAT;
|
||
return resultPose;
|
||
}
|
||
|
||
//产生扫描数据副本和水平扫描数据
|
||
std::vector< std::vector<SVzNL3DPosition>> scanLines;
|
||
scanLines.resize(lineNum);
|
||
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++)
|
||
{
|
||
scanLines[line].insert(scanLines[line].end(), src_scanLines[line].begin(), src_scanLines[line].end());
|
||
for (int j = 0; j < linePtNum; j++)
|
||
{
|
||
scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
|
||
src_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;
|
||
}
|
||
|
||
//算法流程：
|
||
//1、检查垂直方向数据并去除
|
||
//2、聚类
|
||
//3、保留最前面目标
|
||
//4、提取孔
|
||
//5、拟合
|
||
//6、计算中间坐标
|
||
//内部参数
|
||
SSG_cornerParam removeVertialPara = cornerPara;
|
||
removeVertialPara.scale = 3.0;
|
||
removeVertialPara.cornerTh = 60;
|
||
|
||
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<std::vector<int>> zVertivalFlags;
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
if (line == 700)
|
||
int kkk = 1;
|
||
std::vector<int> line_verticalFlags;
|
||
wd_getXYVertialFeature_dirAngleMethod(
|
||
scanLines[line],
|
||
line,
|
||
removeVertialPara,
|
||
line_verticalFlags
|
||
);
|
||
zVertivalFlags.push_back(line_verticalFlags);
|
||
|
||
for (int i = 0; i < (int)line_verticalFlags.size(); i++)
|
||
{
|
||
if (line_verticalFlags[i] > 0)
|
||
flags[line][i] = 1;
|
||
}
|
||
}
|
||
|
||
std::vector<std::vector<int>> zVertivalFlags_h;
|
||
for (int line = 0; line < linePtNum; line++)
|
||
{
|
||
if (line == 1177)
|
||
int kkk = 1;
|
||
std::vector<int> line_verticalFlags;
|
||
wd_getXYVertialFeature_dirAngleMethod(
|
||
scanLines_h[line],
|
||
line,
|
||
removeVertialPara,
|
||
line_verticalFlags
|
||
);
|
||
zVertivalFlags_h.push_back(line_verticalFlags);
|
||
|
||
for (int i = 0; i < (int)line_verticalFlags.size(); i++)
|
||
{
|
||
if (line_verticalFlags[i] > 0)
|
||
flags[i][line] = 1;
|
||
}
|
||
}
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
for (int j = 0; j < linePtNum; j++)
|
||
{
|
||
if (flags[line][j] > 0)
|
||
{
|
||
scanLines[line][j].pt3D.z = 0;
|
||
scanLines_h[j][line].pt3D.z = 0;
|
||
}
|
||
}
|
||
}
|
||
//迭代一次
|
||
SSG_lineSegParam lineSegPara;
|
||
lineSegPara.distScale = 5.0;
|
||
lineSegPara.segGapTh_y = 5.0;
|
||
lineSegPara.segGapTh_z = 5.0;
|
||
const int minSegLen = 5;
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
std::vector<SSG_RUN> segs;
|
||
wd_getLineDataIntervals(
|
||
scanLines[line],
|
||
lineSegPara,
|
||
segs);
|
||
for (int i = 0; i < (int)segs.size(); i++)
|
||
{
|
||
if (segs[i].len <= minSegLen)
|
||
{
|
||
int idx0 = segs[i].start;
|
||
for (int j = 0; j < segs[i].len; j++)
|
||
flags[line][idx0 + j] = 1;
|
||
}
|
||
}
|
||
}
|
||
for (int line = 0; line < linePtNum; line++)
|
||
{
|
||
std::vector<SSG_RUN> segs;
|
||
wd_getLineDataIntervals(
|
||
scanLines_h[line],
|
||
lineSegPara,
|
||
segs);
|
||
for (int i = 0; i < (int)segs.size(); i++)
|
||
{
|
||
if (segs[i].len <= minSegLen)
|
||
{
|
||
int idx0 = segs[i].start;
|
||
for (int j = 0; j < segs[i].len; j++)
|
||
flags[idx0 + j][line] = 1;
|
||
}
|
||
}
|
||
}
|
||
|
||
//标注
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
for (int j = 0; j < linePtNum; j++)
|
||
scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
|
||
}
|
||
//将垂直线段去除
|
||
std::vector< SVzNL3DPosition> validPoints;
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
for (int j = 0; j < linePtNum; j++)
|
||
{
|
||
if (flags[line][j] > 0)
|
||
scanLines[line][j].pt3D.z = 0;
|
||
|
||
if (scanLines[line][j].pt3D.z > 1e-4)
|
||
{
|
||
SVzNL3DPosition a_vldPt;
|
||
a_vldPt.pt3D = scanLines[line][j].pt3D;
|
||
a_vldPt.nPointIdx = (line << 16) | (j & 0xffff);
|
||
validPoints.push_back(a_vldPt);
|
||
}
|
||
}
|
||
}
|
||
|
||
//聚类
|
||
int clusterCheckWin = 5;
|
||
double clusterDist = 5.0;
|
||
int distType = 1; //0 - 2d distance; 1- 3d distance
|
||
std::vector<std::vector< SVzNL3DPosition>> objClusters; //result
|
||
wd_pointClustering_speedUp(
|
||
validPoints,
|
||
lineNum, linePtNum, clusterCheckWin, //搜索窗口
|
||
clusterDist,
|
||
distType,
|
||
objClusters //result
|
||
);
|
||
|
||
//保留最前面的目标
|
||
const double topPlateMinW = 800.0;
|
||
const double topPlateMinH = 800.0;
|
||
const double topPlateMaxZRange = 100.0;
|
||
std::vector<double> objMeanZ;
|
||
std::vector<SVzNLRangeD> objZRange;
|
||
objMeanZ.resize(objClusters.size());
|
||
objZRange.resize(objClusters.size());
|
||
for (int i = 0; i < (int)objClusters.size(); i++)
|
||
{
|
||
SSG_ROIRectD a_roi = _getListROI(objClusters[i]);
|
||
double w = a_roi.right - a_roi.left;
|
||
double h = a_roi.bottom - a_roi.top;
|
||
if ((w > topPlateMinW) && (h > topPlateMinH))
|
||
{
|
||
SVzNLRangeD zRange;
|
||
double meanZ = _getListMeanZ(objClusters[i], zRange);
|
||
objMeanZ[i] = meanZ;
|
||
objZRange[i] = zRange;
|
||
}
|
||
else
|
||
{
|
||
objMeanZ[i] = 0;
|
||
objZRange[i].max = -1.0;
|
||
objZRange[i].min = 0.0;
|
||
}
|
||
}
|
||
|
||
//选出z最小的目标作为顶部轮廓
|
||
int objIdx = -1;
|
||
double minMeanZ = DBL_MAX;
|
||
for (int i = 0; i < (int)objClusters.size(); i++)
|
||
{
|
||
if ((objMeanZ[i] > 1e-4) && (objZRange[i].max > 0))
|
||
{
|
||
double range = objZRange[i].max - objZRange[i].min;
|
||
if (range < topPlateMaxZRange)
|
||
{
|
||
if (minMeanZ > objMeanZ[i])
|
||
{
|
||
minMeanZ = objMeanZ[i];
|
||
objIdx = i;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
if (objIdx < 0)
|
||
{
|
||
*errCode = SG_ERR_ZERO_OBJECTS;
|
||
return resultPose;
|
||
}
|
||
|
||
std::vector< SVzNL3DPosition>& topCluster = objClusters[objIdx];
|
||
//标注
|
||
//重新将flags设置为目标的mask
|
||
for (int i = 0; i < lineNum; i++)
|
||
std::fill(flags[i].begin(), flags[i].end(), -1);
|
||
for (int i = 0; i < (int)topCluster.size(); i++)
|
||
{
|
||
int line = topCluster[i].nPointIdx >> 16;
|
||
int ptIdx = topCluster[i].nPointIdx & 0x0000FFFF;
|
||
scanLines[line][ptIdx].nPointIdx = 2;
|
||
flags[line][ptIdx] = i; //indexing
|
||
}
|
||
|
||
//拟合平面(此处不使用PCA方法，原因是PCA是整体优化，噪点会影响结果）
|
||
std::vector<cv::Point3d> Points3ds;
|
||
for (int i = 0; i < (int)objClusters[objIdx].size(); i++)
|
||
{
|
||
cv::Point3d a_pt = cv::Point3d(objClusters[objIdx][i].pt3D.x, objClusters[objIdx][i].pt3D.y, objClusters[objIdx][i].pt3D.z);
|
||
Points3ds.push_back(a_pt);
|
||
}
|
||
//计算面参数: z = Ax + By + C
|
||
//res: [0]=A, [1]= B, [2]=-1.0, [3]=C,
|
||
std::vector<cv::Point3d> 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;
|
||
}
|
||
|
||
//计算投影向量
|
||
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<SVzNL3DPosition> projectPosition3ds;
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
for (int j = 0; j < linePtNum; j++)
|
||
{
|
||
if (src_scanLines[line][j].pt3D.z > 1e-4)
|
||
{
|
||
SVzNL3DPosition a_vldPt;
|
||
//a_vldPt.pt3D = scanLines[line][j].pt3D;
|
||
a_vldPt.nPointIdx = (line << 16) | (j & 0xffff);
|
||
a_vldPt.pt3D = wd_ptRotate(src_scanLines[line][j].pt3D, poseR.planeCalib);
|
||
projectPosition3ds.push_back(a_vldPt);
|
||
}
|
||
}
|
||
}
|
||
std::vector<SVzNL3DPosition> projectCluster3ds;
|
||
for (int i = 0; i < (int)topCluster.size(); i++)
|
||
{
|
||
SVzNL3DPosition a_pt;
|
||
a_pt.nPointIdx = topCluster[i].nPointIdx;
|
||
a_pt.pt3D = wd_ptRotate(topCluster[i].pt3D, poseR.planeCalib);
|
||
projectCluster3ds.push_back(a_pt);
|
||
}
|
||
//计算meanZ和ROI
|
||
SSG_ROIRectD topCluster_roi2D = _getListROI(topCluster);
|
||
SVzNLRangeD topCluster_zRange;
|
||
double meanZ = _getListMeanZ(topCluster, topCluster_zRange);
|
||
//（1）取Cluster的ROI
|
||
double xw = topCluster_roi2D.right - topCluster_roi2D.left;
|
||
double yw = topCluster_roi2D.bottom - topCluster_roi2D.top;
|
||
double minW = xw < yw ? yw : xw;
|
||
double centerROI_w = (minW / 2) * sqrt(2.0) * 0.9;
|
||
double center_x = (topCluster_roi2D.right + topCluster_roi2D.left) / 2;
|
||
double center_y = (topCluster_roi2D.bottom + topCluster_roi2D.top) / 2;
|
||
//取中央一个小的ROI，去掉边框。这样可以使用PCA方法计算条纹方向
|
||
double sampleROI_L = center_x - centerROI_w / 2;
|
||
double sampleROI_R = center_x + centerROI_w / 2;
|
||
double sampleROI_T = center_y - centerROI_w / 2;
|
||
double sampleROI_B = center_y + centerROI_w / 2;
|
||
|
||
std::vector< SVzNL3DPosition> roi3DPoints;
|
||
std::vector< SVzNL2DPoint> roiPointPostions;
|
||
SVzNL3DPoint roiCentroid = { 0.0, 0.0, 0.0 };
|
||
int sumCounter = 0;
|
||
for (int i = 0; i < (int)projectCluster3ds.size(); i++)
|
||
{
|
||
SVzNL3DPosition& a_pt = projectCluster3ds[i];
|
||
if ((a_pt.pt3D.x > sampleROI_L) && (a_pt.pt3D.x < sampleROI_R) &&
|
||
(a_pt.pt3D.y > sampleROI_T) && (a_pt.pt3D.y < sampleROI_B))
|
||
{
|
||
roi3DPoints.push_back(a_pt);
|
||
|
||
roiCentroid.x += a_pt.pt3D.x;
|
||
roiCentroid.y += a_pt.pt3D.y;
|
||
roiCentroid.z += a_pt.pt3D.z;
|
||
sumCounter++;
|
||
|
||
int line = a_pt.nPointIdx >> 16;
|
||
int ptIdx = a_pt.nPointIdx & 0x0000FFFF;
|
||
SVzNL2DPoint a_position = {line, ptIdx};
|
||
roiPointPostions.push_back(a_position);
|
||
}
|
||
}
|
||
if (sumCounter == 0)
|
||
{
|
||
*errCode = SX_ERR_ZERO_OBJECTS;
|
||
return resultPose;
|
||
}
|
||
roiCentroid.x = roiCentroid.x / sumCounter;
|
||
roiCentroid.y = roiCentroid.y / sumCounter;
|
||
roiCentroid.z = roiCentroid.z / sumCounter;
|
||
|
||
//聚类出其中一条格栅
|
||
std::vector<std::vector< SVzNL3DPosition>> roiClusters; //result
|
||
wd_pointClustering_speedUp(
|
||
roi3DPoints,
|
||
lineNum, linePtNum, clusterCheckWin, //搜索窗口
|
||
clusterDist,
|
||
distType,
|
||
roiClusters //result
|
||
);
|
||
if (roiClusters.size() == 0)
|
||
{
|
||
*errCode = SX_ERR_ZERO_OBJECTS;
|
||
return resultPose;
|
||
}
|
||
std::vector<SSG_ROIRectD> gridROIs;
|
||
int centralGridIdx = -1;
|
||
double minDist = DBL_MAX;
|
||
for (int i = 0; i < (int)roiClusters.size(); i++)
|
||
{
|
||
SSG_ROIRectD a_roi = _getListROI(roiClusters[i]);
|
||
gridROIs.push_back(a_roi);
|
||
|
||
double cx = (a_roi.left + a_roi.right) / 2;
|
||
double cy = (a_roi.top + a_roi.bottom) / 2;
|
||
double dist = sqrt(pow(cx - roiCentroid.x, 2) + pow(cy - roiCentroid.y, 2));
|
||
if (minDist > dist)
|
||
{
|
||
minDist = dist;
|
||
centralGridIdx = i;
|
||
}
|
||
}
|
||
//取中间条纹
|
||
std::vector< SVzNL3DPosition>& centeralCluster = roiClusters[centralGridIdx];
|
||
std::vector< SVzNL2DPointD> roiPoints;
|
||
for (int i = 0; i < (int)centeralCluster.size(); i++)
|
||
{
|
||
SVzNL2DPointD a_pt = { centeralCluster[i].pt3D.x, centeralCluster[i].pt3D.y };
|
||
roiPoints.push_back(a_pt);
|
||
}
|
||
SVzNL2DPointD roi_axis, roi_centroid;
|
||
pca2D(roiPoints, roi_axis, roi_centroid);
|
||
|
||
//(1)cluster继续生长
|
||
//在未旋转的点云中继续生长（端面可能在去除水平点中被去除）
|
||
SVzNL3DRangeD growingROI;
|
||
growingROI.xRange.min = topCluster_roi2D.left - 100.0;
|
||
growingROI.xRange.max = topCluster_roi2D.right + 100.0;
|
||
growingROI.yRange.min = topCluster_roi2D.top - 100.0;
|
||
growingROI.yRange.max = topCluster_roi2D.bottom + 100.0;
|
||
growingROI.zRange.min = 100.0;
|
||
growingROI.zRange.max = topCluster_zRange.max;
|
||
std::vector< SVzNL3DPosition> added_points;
|
||
clusterDist = 10.0;
|
||
wd_clusterGrowing_speedUp(
|
||
projectPosition3ds,
|
||
projectCluster3ds,
|
||
growingROI, //聚类范围，用于加速
|
||
lineNum, linePtNum, clusterCheckWin, //搜索窗口
|
||
clusterDist,
|
||
distType, //0 - 2d distance; 1- 3d distance
|
||
added_points
|
||
);
|
||
|
||
//标注
|
||
for (int i = 0; i < (int)projectCluster3ds.size(); i++)
|
||
{
|
||
int line = projectCluster3ds[i].nPointIdx >> 16;
|
||
int ptIdx = projectCluster3ds[i].nPointIdx & 0x0000FFFF;
|
||
src_scanLines[line][ptIdx].nPointIdx = 2;
|
||
}
|
||
for (int i = 0; i < (int)roiPointPostions.size(); i++)
|
||
{
|
||
int line = roiPointPostions[i].x;
|
||
int ptIdx = roiPointPostions[i].y;
|
||
src_scanLines[line][ptIdx].nPointIdx = 3;
|
||
}
|
||
|
||
|
||
//搜索4个角点
|
||
SVzNL2DPointD rotate_axis1 = wd_pt2dRotate(roi_axis, 45);
|
||
double denominator = sqrt(pow(rotate_axis1.x, 2) + pow(rotate_axis1.y, 2));
|
||
rotate_axis1 = { rotate_axis1.x/ denominator , rotate_axis1.y /denominator };
|
||
|
||
SVzNL2DPointD rotate_axis2 = wd_pt2dRotate(roi_axis, -45);
|
||
denominator = sqrt(pow(rotate_axis2.x, 2) + pow(rotate_axis2.y, 2));
|
||
rotate_axis2 = { rotate_axis2.x / denominator , rotate_axis2.y / denominator };
|
||
|
||
//生成过roi_centroid的两条直线
|
||
//A = dy, B = −dx, C = dx⋅y0−dy⋅x0
|
||
double line1_A, line1_B, line1_C;
|
||
line1_A = rotate_axis1.y;
|
||
line1_B = -rotate_axis1.x;
|
||
line1_C = roi_centroid.y * rotate_axis1.x - roi_centroid.x * rotate_axis1.y;
|
||
double line2_A, line2_B, line2_C;
|
||
line2_A = rotate_axis2.y;
|
||
line2_B = -rotate_axis2.x;
|
||
line2_C = roi_centroid.y * rotate_axis2.x - roi_centroid.x * rotate_axis2.y;
|
||
//寻找四个顶点：距两条直线（line1和line2）距离最大的点
|
||
SVzNL3DPosition corner1, corner2, corner3, corner4;
|
||
double maxDist1 = 0.0, maxDist2 = DBL_MAX, maxDist3 = 0.0, maxDist4 = DBL_MAX;
|
||
for (int i = 0; i < (int)projectCluster3ds.size(); i++)
|
||
{
|
||
SVzNL3DPosition& a_pt = projectCluster3ds[i];
|
||
double dist1 = line1_A * a_pt.pt3D.x + line1_B * a_pt.pt3D.y + line1_C;
|
||
double dist2 = line2_A * a_pt.pt3D.x + line2_B * a_pt.pt3D.y + line2_C;
|
||
if (maxDist1 < dist1) //距离最大值
|
||
{
|
||
maxDist1 = dist1;
|
||
corner1 = a_pt;
|
||
}
|
||
if (maxDist2 > dist1) //距离最小值, 负值
|
||
{
|
||
maxDist2 = dist1;
|
||
corner2 = a_pt;
|
||
}
|
||
if (maxDist3 < dist2) //距离最大值
|
||
{
|
||
maxDist3 = dist2;
|
||
corner3 = a_pt;
|
||
}
|
||
if (maxDist4 > dist2) //距离最小值, 负值
|
||
{
|
||
maxDist4 = dist2;
|
||
corner4 = a_pt;
|
||
}
|
||
}
|
||
//计算两条对角线
|
||
double crossLine1_A, crossLine1_B, crossLine1_C;
|
||
compute2ptLine(
|
||
corner1.pt3D,
|
||
corner2.pt3D,
|
||
&crossLine1_A, &crossLine1_B, &crossLine1_C);
|
||
double crossLine2_A, crossLine2_B, crossLine2_C;
|
||
compute2ptLine(
|
||
corner3.pt3D,
|
||
corner4.pt3D,
|
||
&crossLine2_A, &crossLine2_B, &crossLine2_C);
|
||
//计算对角线交点
|
||
SVzNL3DPoint rackCenter = computeLineCrossPt_abs(
|
||
crossLine1_A, crossLine1_B, crossLine1_C,
|
||
crossLine2_A, crossLine2_B, crossLine2_C);
|
||
rackCenter.z = meanZ;
|
||
//生成结果
|
||
resultPose.point = rackCenter;
|
||
resultPose.pose_x = { roi_axis.x, roi_axis.y, 0 };
|
||
resultPose.pose_z = { 0.0, 0.0, 1.0 };
|
||
//叉乘出y
|
||
resultPose.pose_y = vec3_cross(resultPose.pose_z, resultPose.pose_x);
|
||
|
||
//旋转回原坐标系
|
||
resultPose.point = _ptRotate(resultPose.point, poseR.invRMatrix);
|
||
resultPose.pose_x = _ptRotate(resultPose.pose_x, poseR.invRMatrix);
|
||
resultPose.pose_y = _ptRotate(resultPose.pose_y, poseR.invRMatrix);
|
||
resultPose.pose_z = _ptRotate(resultPose.pose_z, poseR.invRMatrix);
|
||
return resultPose;
|
||
}
|
||
|
||
int _getMaxROI(std::vector<SSG_ROIRectD>& roiList)
|
||
{
|
||
if (roiList.size() == 0)
|
||
return -1;
|
||
|
||
int maxId = 0;
|
||
double maxSize = (roiList[0].right - roiList[0].left) * (roiList[0].bottom - roiList[0].top);
|
||
for (int i = 1; i < (int)roiList.size(); i++)
|
||
{
|
||
double size = (roiList[i].right - roiList[i].left) * (roiList[i].bottom - roiList[i].top);
|
||
if (maxSize < size)
|
||
{
|
||
maxId = i;
|
||
maxSize = size;
|
||
}
|
||
}
|
||
return maxId;
|
||
}
|
||
|
||
void _getFlagIntervals_v(
|
||
std::vector<std::vector<int>>& flags,
|
||
int index,
|
||
const int maxSkipping,
|
||
std::vector<SSG_RUN>& segs)
|
||
{
|
||
SSG_RUN a_run = { 0, -1, 0 }; //startIdx, len, lastIdx
|
||
int dataSize = (int)flags[index].size();
|
||
for (int i = 0; i < dataSize; i++)
|
||
{
|
||
if (flags[index][i] >=0 )
|
||
{
|
||
if (a_run.len < 0)
|
||
{
|
||
a_run.start = i;
|
||
a_run.len = 1;
|
||
a_run.value = flags[index][i];
|
||
}
|
||
else
|
||
{
|
||
int gap = i - (a_run.start + a_run.len - 1);
|
||
if( (flags[index][i] == a_run.value) && (gap <= maxSkipping))
|
||
{
|
||
a_run.len = i - a_run.start + 1;
|
||
}
|
||
else
|
||
{
|
||
segs.push_back(a_run);
|
||
|
||
a_run.start = i;
|
||
a_run.len = 1;
|
||
a_run.value = a_run.value = flags[index][i];
|
||
}
|
||
}
|
||
}
|
||
}
|
||
if (a_run.len > 0)
|
||
segs.push_back(a_run);
|
||
}
|
||
|
||
void _getFlagIntervals_h(
|
||
std::vector<std::vector<int>>& flags,
|
||
int index,
|
||
const int maxSkipping,
|
||
std::vector<SSG_RUN>& segs)
|
||
{
|
||
SSG_RUN a_run = { 0, -1, 0 }; //startIdx, len, lastIdx
|
||
int dataSize = (int)flags.size();
|
||
for (int i = 0; i < dataSize; i++)
|
||
{
|
||
if (flags[i][index] >= 0)
|
||
{
|
||
if (a_run.len < 0)
|
||
{
|
||
a_run.start = i;
|
||
a_run.len = 1;
|
||
a_run.value = flags[i][index];
|
||
}
|
||
else
|
||
{
|
||
int gap = i - (a_run.start + a_run.len - 1);
|
||
if ((flags[i][index] == a_run.value) && (gap <= maxSkipping))
|
||
{
|
||
a_run.len = i - a_run.start + 1;
|
||
}
|
||
else
|
||
{
|
||
segs.push_back(a_run);
|
||
|
||
a_run.start = i;
|
||
a_run.len = 1;
|
||
a_run.value = a_run.value = flags[i][index];
|
||
}
|
||
}
|
||
}
|
||
}
|
||
if (a_run.len > 0)
|
||
segs.push_back(a_run);
|
||
}
|
||
|
||
void _computeAnlgeScanData(
|
||
std::vector< SVzNL3DPosition>& clusterPoints,
|
||
const double angleScale,
|
||
const SVzNL3DPoint& centerPoint,
|
||
std::vector<std::vector<SWD_polarPt>>& polarScanData)
|
||
{
|
||
int dataSize = (int)clusterPoints.size();
|
||
for (int i = 0; i < dataSize; i++)
|
||
{
|
||
int line = clusterPoints[i].nPointIdx >> 16;
|
||
int ptIdx = clusterPoints[i].nPointIdx & 0x0000FFFF;
|
||
SVzNL3DPoint& a_pt = clusterPoints[i].pt3D;
|
||
|
||
double angle = atan2(a_pt.y - centerPoint.y, a_pt.x - centerPoint.x);
|
||
angle = (angle / PI) * 180 + 180.0;
|
||
double R = sqrt(pow(a_pt.y - centerPoint.y, 2) + pow(a_pt.x - centerPoint.x, 2));
|
||
|
||
int angleLine = (int)(angle / angleScale);
|
||
|
||
SWD_polarPt a_polarPt;
|
||
a_polarPt.lineIdx = line;
|
||
a_polarPt.ptIdx = ptIdx;
|
||
a_polarPt.x = a_pt.x;
|
||
a_polarPt.y = a_pt.y;
|
||
a_polarPt.z = a_pt.z;
|
||
a_polarPt.R = R;
|
||
a_polarPt.angle = angle;
|
||
polarScanData[angleLine].push_back(a_polarPt);
|
||
}
|
||
}
|
||
bool compareByPolarScanR(const SWD_polarPt& a, const SWD_polarPt& b) {
|
||
return a.R < b.R;
|
||
}
|
||
bool _compareByTireZValue(WD_HolePositionInfo& a, WD_HolePositionInfo& b)
|
||
{
|
||
return a.center.z < b.center.z;
|
||
}
|
||
//山东本事机电轮胎中心定位
|
||
void sx_getTireHolePose(
|
||
std::vector< std::vector<SVzNL3DPosition>>& scanLinesInput,
|
||
const SSG_cornerParam cornerPara,
|
||
const SSG_planeCalibPara groundCalibPara,
|
||
const WD_tireParam tireParam,
|
||
std::vector<WD_HolePositionInfo>& tirePositions,
|
||
int* errCode)
|
||
{
|
||
*errCode = 0;
|
||
|
||
//生成数据副本，使用副本数据进行调平和后续处理
|
||
int lineNum = (int)scanLinesInput.size();
|
||
std::vector< std::vector<SVzNL3DPosition>> scanLines;
|
||
scanLines.resize(lineNum);
|
||
int linePtNum = (int)scanLinesInput[0].size();
|
||
bool isGridData = true;
|
||
for (int i = 0; i < lineNum; i++)
|
||
{
|
||
if (linePtNum != (int)scanLinesInput[i].size())
|
||
isGridData = false;
|
||
|
||
scanLines[i].resize(scanLinesInput[i].size());
|
||
std::copy(scanLinesInput[i].begin(), scanLinesInput[i].end(), scanLines[i].begin()); // 使用std::copy算法
|
||
|
||
for (int j = 0; j < (int)scanLinesInput[i].size(); j++)
|
||
scanLinesInput[i][j].nPointIdx = 0; //清零，用于debug时记录信息
|
||
}
|
||
if (false == isGridData)//数据不是网格格式
|
||
{
|
||
*errCode = SG_ERR_NOT_GRID_FORMAT;
|
||
return;
|
||
}
|
||
|
||
double removeGroundHeight = groundCalibPara.planeHeight - tireParam.thickness * 0.1;
|
||
for (int i = 0; i < lineNum; i++)
|
||
{ //行处理
|
||
//调平，去除地面
|
||
wd_lineDataR(scanLines[i], groundCalibPara.planeCalib, removeGroundHeight);
|
||
}
|
||
|
||
//产生水平扫描数据
|
||
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;
|
||
}
|
||
|
||
//算法流程：
|
||
//1、检查垂直方向数据并去除
|
||
//2、聚类
|
||
//3、保留最前面目标
|
||
//4、提取孔
|
||
//5、拟合
|
||
//6、计算中间坐标
|
||
//内部参数
|
||
SSG_cornerParam removeVertialPara = cornerPara;
|
||
removeVertialPara.scale = 10.0;
|
||
removeVertialPara.cornerTh = 60;
|
||
|
||
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<std::vector<int>> zVertivalFlags;
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
if (line == 700)
|
||
int kkk = 1;
|
||
std::vector<int> line_verticalFlags;
|
||
wd_getXYVertialFeature_dirAngleMethod(
|
||
scanLines[line],
|
||
line,
|
||
removeVertialPara,
|
||
line_verticalFlags
|
||
);
|
||
zVertivalFlags.push_back(line_verticalFlags);
|
||
|
||
for (int i = 0; i < (int)line_verticalFlags.size(); i++)
|
||
{
|
||
if (line_verticalFlags[i] > 0)
|
||
flags[line][i] = 1;
|
||
}
|
||
}
|
||
|
||
std::vector<std::vector<int>> zVertivalFlags_h;
|
||
for (int line = 0; line < linePtNum; line++)
|
||
{
|
||
if (line == 1177)
|
||
int kkk = 1;
|
||
std::vector<int> line_verticalFlags;
|
||
wd_getXYVertialFeature_dirAngleMethod(
|
||
scanLines_h[line],
|
||
line,
|
||
removeVertialPara,
|
||
line_verticalFlags
|
||
);
|
||
zVertivalFlags_h.push_back(line_verticalFlags);
|
||
|
||
for (int i = 0; i < (int)line_verticalFlags.size(); i++)
|
||
{
|
||
if (line_verticalFlags[i] > 0)
|
||
flags[i][line] = 1;
|
||
}
|
||
}
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
for (int j = 0; j < linePtNum; j++)
|
||
{
|
||
if (flags[line][j] > 0)
|
||
{
|
||
scanLines[line][j].pt3D.z = 0;
|
||
scanLines_h[j][line].pt3D.z = 0;
|
||
}
|
||
}
|
||
}
|
||
//迭代一次
|
||
SSG_lineSegParam lineSegPara;
|
||
lineSegPara.distScale = 5.0;
|
||
lineSegPara.segGapTh_y = 5.0;
|
||
lineSegPara.segGapTh_z = 5.0;
|
||
const int minSegLen = 5;
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
std::vector<SSG_RUN> segs;
|
||
wd_getLineDataIntervals(
|
||
scanLines[line],
|
||
lineSegPara,
|
||
segs);
|
||
for (int i = 0; i < (int)segs.size(); i++)
|
||
{
|
||
if (segs[i].len <= minSegLen)
|
||
{
|
||
int idx0 = segs[i].start;
|
||
for (int j = 0; j < segs[i].len; j++)
|
||
flags[line][idx0 + j] = 1;
|
||
}
|
||
}
|
||
}
|
||
for (int line = 0; line < linePtNum; line++)
|
||
{
|
||
std::vector<SSG_RUN> segs;
|
||
wd_getLineDataIntervals(
|
||
scanLines_h[line],
|
||
lineSegPara,
|
||
segs);
|
||
for (int i = 0; i < (int)segs.size(); i++)
|
||
{
|
||
if (segs[i].len <= minSegLen)
|
||
{
|
||
int idx0 = segs[i].start;
|
||
for (int j = 0; j < segs[i].len; j++)
|
||
flags[idx0 + j][line] = 1;
|
||
}
|
||
}
|
||
}
|
||
|
||
//标注
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
for (int j = 0; j < linePtNum; j++)
|
||
{
|
||
scanLinesInput[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
|
||
scanLines[line][j].nPointIdx = 0;
|
||
}
|
||
}
|
||
//将垂直线段去除
|
||
std::vector< SVzNL3DPosition> validPoints;
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
for (int j = 0; j < linePtNum; j++)
|
||
{
|
||
if (flags[line][j] > 0)
|
||
scanLines[line][j].pt3D.z = 0;
|
||
|
||
if (scanLines[line][j].pt3D.z > 1e-4)
|
||
{
|
||
SVzNL3DPosition a_vldPt;
|
||
a_vldPt.pt3D = scanLines[line][j].pt3D;
|
||
a_vldPt.nPointIdx = (line << 16) | (j & 0xffff);
|
||
validPoints.push_back(a_vldPt);
|
||
}
|
||
}
|
||
}
|
||
|
||
//聚类
|
||
int clusterCheckWin = 5;
|
||
double clusterDist = 5.0;
|
||
int distType = 1; //0 - 2d distance; 1- 3d distance
|
||
std::vector<std::vector< SVzNL3DPosition>> objClusters; //result
|
||
wd_pointClustering_speedUp(
|
||
validPoints,
|
||
lineNum, linePtNum, clusterCheckWin, //搜索窗口
|
||
clusterDist,
|
||
distType,
|
||
objClusters //result
|
||
);
|
||
|
||
//保留最前面的目标
|
||
double minObjSize = tireParam.diameter * 0.8;
|
||
double maxObjSize = tireParam.diameter * 1.5;
|
||
const double topPlateMaxZRange = 100.0;
|
||
std::vector<double> objMeanZ;
|
||
std::vector<SSG_ROIRectD> objROIs;
|
||
std::vector<SVzNLRangeD> objZRange;
|
||
objMeanZ.resize(objClusters.size());
|
||
objROIs.resize(objClusters.size());
|
||
objZRange.resize(objClusters.size());
|
||
std::vector<int> validObjIndice;
|
||
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;
|
||
if ( (w > minObjSize) && (h > minObjSize) && (w < maxObjSize) && (h < maxObjSize))
|
||
{
|
||
SVzNLRangeD zRange;
|
||
double meanZ = _getListMeanZ(objClusters[i], zRange);
|
||
objMeanZ[i] = meanZ;
|
||
objZRange[i] = zRange;
|
||
validObjIndice.push_back(i);
|
||
}
|
||
else
|
||
{
|
||
objMeanZ[i] = 0;
|
||
objZRange[i].max = -1.0;
|
||
objZRange[i].min = 0.0;
|
||
}
|
||
}
|
||
|
||
if (validObjIndice.size() == 0)
|
||
{
|
||
*errCode = SG_ERR_ZERO_OBJECTS;
|
||
return;
|
||
}
|
||
|
||
//标注
|
||
//重新将flags设置为目标的mask
|
||
for (int i = 0; i < lineNum; i++)
|
||
std::fill(flags[i].begin(), flags[i].end(), -1);
|
||
for (int m = 0; m < (int)validObjIndice.size(); m++)
|
||
{
|
||
int objIdx = validObjIndice[m];
|
||
for (int i = 0; i < (int)objClusters[objIdx].size(); i++)
|
||
{
|
||
int line = objClusters[objIdx][i].nPointIdx >> 16;
|
||
int ptIdx = objClusters[objIdx][i].nPointIdx & 0x0000FFFF;
|
||
scanLinesInput[line][ptIdx].nPointIdx = 2;
|
||
flags[line][ptIdx] = m; //indexing
|
||
}
|
||
}
|
||
//提取轮廓点
|
||
|
||
#if 1
|
||
//按照圆扫描获取轮廓点
|
||
//计算质心
|
||
for (int m = 0; m < (int)validObjIndice.size(); m++)
|
||
{
|
||
int objIdx = validObjIndice[m];
|
||
SVzNL3DPoint centroid = { 0, 0, 0 };
|
||
int cluster_size = (int)objClusters[objIdx].size();
|
||
for (int i = 0; i < cluster_size; i++)
|
||
{
|
||
int line = objClusters[objIdx][i].nPointIdx >> 16;
|
||
int ptIdx = objClusters[objIdx][i].nPointIdx & 0x0000FFFF;
|
||
centroid.x += scanLinesInput[line][ptIdx].pt3D.x;
|
||
centroid.y += scanLinesInput[line][ptIdx].pt3D.y;
|
||
centroid.z += scanLinesInput[line][ptIdx].pt3D.z;
|
||
}
|
||
centroid.x = centroid.x / cluster_size;
|
||
centroid.y = centroid.y / cluster_size;
|
||
centroid.z = centroid.z / cluster_size;
|
||
|
||
//圆周扫描:0.25度间隔
|
||
double angleScale = 1.0;
|
||
std::vector<std::vector<SWD_polarPt>> polarScanData;
|
||
int polarLines = (int)(360.0 / angleScale + 0.5);
|
||
polarScanData.resize(polarLines);
|
||
_computeAnlgeScanData(
|
||
objClusters[objIdx],
|
||
angleScale,
|
||
centroid,
|
||
polarScanData);
|
||
|
||
//每个扫描角度按R从小到大排序
|
||
for (int i = 0; i < polarLines; i++)
|
||
{
|
||
std::sort(polarScanData[i].begin(), polarScanData[i].end(), compareByPolarScanR);
|
||
}
|
||
|
||
//提取轮廓
|
||
std::vector< SWD_polarPt> contourPoints;
|
||
std::vector< SVzNL3DPosition> fittingPoints;
|
||
for (int i = 0; i < polarLines; i++)
|
||
{
|
||
if (polarScanData[i].size() == 0)
|
||
continue;
|
||
|
||
contourPoints.push_back(polarScanData[i][0]);
|
||
|
||
SVzNL3DPosition a_pt;
|
||
a_pt.nPointIdx = 0;
|
||
a_pt.pt3D = { polarScanData[i][0].x, polarScanData[i][0].y, polarScanData[i][0].z};
|
||
fittingPoints.push_back(a_pt);
|
||
}
|
||
|
||
//标记
|
||
for (int m = 0; m < (int)contourPoints.size(); m++)
|
||
{
|
||
int line = contourPoints[m].lineIdx;
|
||
int ptIdx = contourPoints[m].ptIdx;
|
||
scanLinesInput[line][ptIdx].nPointIdx = 3;
|
||
}
|
||
|
||
//提取中心点
|
||
SVzNLRangeD holezRange;
|
||
double meanZ = _getListMeanZ(fittingPoints, holezRange);
|
||
//拟合圆心
|
||
//圆最小二乘拟合
|
||
SVzNL3DPoint center = { 0.0, 0.0, 0.0 };
|
||
double radius = 0;
|
||
#if 0
|
||
double errValue = fitCircleByLeastSquare_2(fittingPoints, center, radius);
|
||
#else
|
||
double errValue = fitCircleRobust(fittingPoints, center, radius);
|
||
#endif
|
||
if ((radius > tireParam.diameter * 0.15) && (radius < tireParam.diameter * 0.75))
|
||
{
|
||
WD_HolePositionInfo a_holeInfo;
|
||
a_holeInfo.center = { center.x, center.y, meanZ };
|
||
a_holeInfo.normDir = { 0.0, 0.0, 1.0 };
|
||
a_holeInfo.holeR = radius;
|
||
tirePositions.push_back(a_holeInfo);
|
||
}
|
||
}
|
||
|
||
#else
|
||
int maxSkipping = 3; //间隔小于3的空白视为连续，消除扫描拉条子的影响
|
||
std::vector<std::vector<int>> contourMasks;
|
||
contourMasks.resize(lineNum);
|
||
for (int i = 0; i < lineNum; i++)
|
||
{
|
||
contourMasks[i].resize(linePtNum);
|
||
std::fill(contourMasks[i].begin(), contourMasks[i].end(), -1);
|
||
}
|
||
//垂直方向
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
std::vector<SSG_RUN> flagSegs;
|
||
_getFlagIntervals_v(
|
||
flags,
|
||
line,
|
||
maxSkipping,
|
||
flagSegs);
|
||
for (int j = 0; j < (int)flagSegs.size(); j++)
|
||
{
|
||
int idx1 = flagSegs[j].start;
|
||
int idx2 = flagSegs[j].start + flagSegs[j].len - 1;
|
||
contourMasks[line][idx1] = flagSegs[j].value;
|
||
contourMasks[line][idx2] = flagSegs[j].value;
|
||
}
|
||
}
|
||
//水平方向
|
||
for (int ptIdx = 0; ptIdx < linePtNum; ptIdx++)
|
||
{
|
||
std::vector<SSG_RUN> flagSegs;
|
||
_getFlagIntervals_h(
|
||
flags,
|
||
ptIdx,
|
||
maxSkipping,
|
||
flagSegs);
|
||
for (int j = 0; j < (int)flagSegs.size(); j++)
|
||
{
|
||
int idx1 = flagSegs[j].start;
|
||
int idx2 = flagSegs[j].start + flagSegs[j].len - 1;
|
||
contourMasks[idx1][ptIdx] = flagSegs[j].value;
|
||
contourMasks[idx2][ptIdx] = flagSegs[j].value;
|
||
}
|
||
}
|
||
std::vector<std::vector< SVzNL3DPosition>> contourPoints;
|
||
contourPoints.resize(validObjIndice.size());
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
for (int ptIdx = 0; ptIdx < linePtNum; ptIdx++)
|
||
{
|
||
if (contourMasks[line][ptIdx] >= 0)
|
||
{
|
||
int cIdx = contourMasks[line][ptIdx];
|
||
SVzNL3DPosition a_vldPt;
|
||
a_vldPt.pt3D = scanLines[line][ptIdx].pt3D;
|
||
a_vldPt.nPointIdx = (line << 16) | (ptIdx & 0xffff);
|
||
contourPoints[cIdx].push_back(a_vldPt);
|
||
}
|
||
}
|
||
}
|
||
//标记
|
||
for (int m = 0; m < (int)contourPoints.size(); m++)
|
||
{
|
||
for (int j = 0; j < contourPoints[m].size(); j++)
|
||
{
|
||
int line = contourPoints[m][j].nPointIdx >> 16;
|
||
int ptIdx = contourPoints[m][j].nPointIdx & 0x0000FFFF;
|
||
scanLinesInput[line][ptIdx].nPointIdx = m + 3;
|
||
}
|
||
}
|
||
|
||
//逐个计算目标姿态
|
||
clusterDist = 10.0; //轮廓点聚类适当加大距离
|
||
distType = 0; //0 - 2d distance; 1- 3d distance
|
||
for (int m = 0; m < (int)contourPoints.size(); m++)
|
||
{
|
||
std::vector<std::vector< SVzNL3DPosition>> contourClusters; //result
|
||
wd_pointClustering_speedUp(
|
||
contourPoints[m],
|
||
lineNum, linePtNum, clusterCheckWin, //搜索窗口
|
||
clusterDist,
|
||
distType,
|
||
contourClusters //result
|
||
);
|
||
//计算ROI
|
||
std::vector<SSG_ROIRectD> coutourROIs;
|
||
for (int i = 0; i < (int)contourClusters.size(); i++)
|
||
{
|
||
SSG_ROIRectD a_roi = _getListROI(contourClusters[i]);
|
||
coutourROIs.push_back(a_roi);
|
||
}
|
||
//去掉最大的目标
|
||
int maxId = _getMaxROI(coutourROIs);
|
||
if (maxId < 0)
|
||
continue;
|
||
double innerHole_w = coutourROIs[maxId].right - coutourROIs[maxId].left;
|
||
double innerHole_h = coutourROIs[maxId].bottom - coutourROIs[maxId].top;
|
||
while ((innerHole_w > minObjSize) || (innerHole_h > minObjSize))
|
||
{
|
||
coutourROIs[maxId].right = 0;
|
||
coutourROIs[maxId].left = 0;
|
||
coutourROIs[maxId].bottom = 0;
|
||
coutourROIs[maxId].top = 0;
|
||
//继续找最大的目标，若大于门限，视为目标
|
||
maxId = _getMaxROI(coutourROIs);
|
||
if (maxId < 0)
|
||
continue;
|
||
|
||
innerHole_w = coutourROIs[maxId].right - coutourROIs[maxId].left;
|
||
innerHole_h = coutourROIs[maxId].bottom - coutourROIs[maxId].top;
|
||
}
|
||
|
||
if ((innerHole_w > tireParam.diameter * 0.4) && (innerHole_h > tireParam.diameter * 0.4))
|
||
{
|
||
//计算姿态
|
||
SVzNLRangeD holezRange;
|
||
double meanZ = _getListMeanZ(contourClusters[maxId], holezRange);
|
||
//拟合圆心
|
||
//圆最小二乘拟合
|
||
SVzNL3DPoint center = { 0.0, 0.0, 0.0 };
|
||
double radius = 0;
|
||
double errValue = fitCircleByLeastSquare_2( contourClusters[maxId], center, radius);
|
||
if ((radius > tireParam.diameter * 0.2) && (radius < minObjSize * 0.5))
|
||
{
|
||
WD_HolePositionInfo a_holeInfo;
|
||
a_holeInfo.center = { center.x, center.y, meanZ };
|
||
a_holeInfo.normDir = { 0.0, 0.0, 1.0 };
|
||
a_holeInfo.holeR = radius;
|
||
tirePositions.push_back(a_holeInfo);
|
||
}
|
||
}
|
||
}
|
||
#endif
|
||
//按高度排序
|
||
std::sort(tirePositions.begin(), tirePositions.end(), _compareByTireZValue);
|
||
|
||
if (tirePositions.size() == 0)
|
||
{
|
||
*errCode = SG_ERR_ZERO_OBJECTS;
|
||
return;
|
||
}
|
||
|
||
//旋转回原坐标系
|
||
for (int i = 0; i < (int)tirePositions.size(); i++)
|
||
{
|
||
tirePositions[i].center = _ptRotate(tirePositions[i].center, groundCalibPara.invRMatrix);
|
||
tirePositions[i].normDir = _ptRotate(tirePositions[i].normDir, groundCalibPara.invRMatrix);
|
||
}
|
||
|
||
return;
|
||
}
|
||
|
||
//模板检查
|
||
void _templateChecking(
|
||
std::vector<std::vector<double>>& quantiData,
|
||
int ref_x, int ref_y,
|
||
std::vector<std::vector<int>>& templateMask,
|
||
std::vector<double>& circleZ,
|
||
std::vector<double>& centerZ)
|
||
{
|
||
int templateX = (int)templateMask.size();
|
||
int templateY = (int)templateMask[0].size();
|
||
for (int x = 0; x < templateX; x++)
|
||
{
|
||
for (int y = 0; y < templateY; y++)
|
||
{
|
||
if (templateMask[x][y] == 0)
|
||
continue;
|
||
int maskX = x + ref_x;
|
||
int maskY = y + ref_y;
|
||
if (quantiData[maskX][maskY] < 1e-4)
|
||
continue;
|
||
|
||
if (templateMask[x][y] == 1)
|
||
circleZ.push_back(quantiData[maskX][maskY]);
|
||
else if (templateMask[x][y] == 2)
|
||
centerZ.push_back(quantiData[maskX][maskY]);
|
||
}
|
||
}
|
||
return;
|
||
}
|
||
|
||
bool validateVarPeak(std::vector<std::vector< SSG_2DValueI>>& varPeaks, SSG_2DValueI& seedPeak, const int searchRng)
|
||
{
|
||
int startX = seedPeak.x - searchRng;
|
||
if (startX < 0)
|
||
startX = 0;
|
||
int endX = seedPeak.x + searchRng;
|
||
if (endX >= (int)varPeaks.size())
|
||
endX = (int)varPeaks.size() - 1;
|
||
|
||
int startY = seedPeak.y - searchRng;
|
||
int endY = seedPeak.y + searchRng;
|
||
bool isPeak = true;
|
||
for (int x = startX; x <= endX; x++)
|
||
{
|
||
int linePkSize = (int)varPeaks[x].size();
|
||
if (linePkSize == 0)
|
||
continue;
|
||
|
||
for (int m = 0; m < linePkSize; m++)
|
||
{
|
||
if (varPeaks[x][m].y < startY)
|
||
continue;
|
||
if (varPeaks[x][m].y > endY)
|
||
break;
|
||
|
||
if ((varPeaks[x][m].x == seedPeak.x) && (varPeaks[x][m].y == seedPeak.y))
|
||
continue;
|
||
|
||
//比较
|
||
if (seedPeak.valueD > varPeaks[x][m].valueD)
|
||
{
|
||
isPeak = false;
|
||
break;
|
||
}
|
||
else if (seedPeak.valueD < varPeaks[x][m].valueD)
|
||
varPeaks[x][m].value = 1;
|
||
}
|
||
if (false == isPeak)
|
||
break;
|
||
}
|
||
return isPeak;
|
||
}
|
||
|
||
//宁波海瑞马定子芯中心定位
|
||
void sx_getRotorCorePose(
|
||
std::vector< std::vector<SVzNL3DPosition>>& scanLinesInput,
|
||
const SSG_cornerParam cornerPara,
|
||
const SSG_planeCalibPara groundCalibPara,
|
||
const WD_rotorAppParam rotorParam,
|
||
std::vector<WD_HolePositionInfo>& rotorPositions,
|
||
int* errCode)
|
||
{
|
||
*errCode = 0;
|
||
|
||
//生成数据副本，使用副本数据进行调平和后续处理
|
||
int lineNum = (int)scanLinesInput.size();
|
||
std::vector< std::vector<SVzNL3DPosition>> scanLines;
|
||
scanLines.resize(lineNum);
|
||
int linePtNum = (int)scanLinesInput[0].size();
|
||
bool isGridData = true;
|
||
for (int i = 0; i < lineNum; i++)
|
||
{
|
||
if (linePtNum != (int)scanLinesInput[i].size())
|
||
isGridData = false;
|
||
|
||
scanLines[i].resize(scanLinesInput[i].size());
|
||
std::copy(scanLinesInput[i].begin(), scanLinesInput[i].end(), scanLines[i].begin()); // 使用std::copy算法
|
||
|
||
for (int j = 0; j < (int)scanLinesInput[i].size(); j++)
|
||
scanLinesInput[i][j].nPointIdx = 0; //清零，用于debug时记录信息
|
||
}
|
||
if (false == isGridData)//数据不是网格格式
|
||
{
|
||
*errCode = SG_ERR_NOT_GRID_FORMAT;
|
||
return;
|
||
}
|
||
|
||
//内部参数
|
||
double removeGroundHeight = groundCalibPara.planeHeight - rotorParam.removeGroundHOffset;
|
||
for (int i = 0; i < lineNum; i++)
|
||
{ //行处理
|
||
//调平，去除地面
|
||
wd_lineDataR(scanLines[i], groundCalibPara.planeCalib, removeGroundHeight);
|
||
}
|
||
|
||
//产生水平扫描数据
|
||
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;
|
||
}
|
||
|
||
//算法流程：
|
||
//1、检查垂直方向数据并去除
|
||
//2、聚类
|
||
//3、保留最前面目标
|
||
//4、提取孔
|
||
//5、拟合
|
||
//6、计算中间坐标
|
||
|
||
//内部参数
|
||
double maxDistTh = 10.0;
|
||
double minSegLen = 2.0;
|
||
SSG_cornerParam removeVertialPara = cornerPara;
|
||
removeVertialPara.scale = 5.0;
|
||
removeVertialPara.cornerTh = 60;
|
||
|
||
SWDRimEdgeParam rimPara_1 = {60, 25};
|
||
SWDRimEdgeParam rimPara_2 = { 60, 25 };
|
||
|
||
SVzNLRangeD rimWidth = { 1,20 };
|
||
|
||
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<std::vector<int>> zVertivalFlags;
|
||
std::vector<std::vector<SSG_RUN>> rimFeatures;
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
if (line == 578)
|
||
int kkk = 1;
|
||
|
||
//根据z连续性分段
|
||
std::vector<SSG_RUN> segs;
|
||
wd_lineDataSegment_dist_2(
|
||
scanLines[line],
|
||
segs,
|
||
maxDistTh,
|
||
minSegLen
|
||
);
|
||
|
||
std::vector<int> line_verticalFlags;
|
||
wd_getXYVertialFeature_perSeg_dirAngleMethod(
|
||
scanLines[line],
|
||
segs,
|
||
line,
|
||
removeVertialPara,
|
||
line_verticalFlags
|
||
);
|
||
zVertivalFlags.push_back(line_verticalFlags);
|
||
//提取边沿特征
|
||
std::vector<SSG_RUN> line_rimFeatures;
|
||
wd_getRimConvexFeature(
|
||
scanLines[line],
|
||
segs,
|
||
line,
|
||
rimPara_1,
|
||
rimPara_2,
|
||
rimWidth,
|
||
line_rimFeatures
|
||
);
|
||
rimFeatures.push_back(line_rimFeatures);
|
||
|
||
for (int i = 0; i < (int)line_verticalFlags.size(); i++)
|
||
{
|
||
if (line_verticalFlags[i] > 0)
|
||
flags[line][i] = 1;
|
||
}
|
||
for (int i = 0; i < (int)line_rimFeatures.size(); i++)
|
||
{
|
||
SSG_RUN& a_rim = line_rimFeatures[i];
|
||
for (int j = 0; j < a_rim.len; j++)
|
||
{
|
||
flags[line][j+a_rim.start] = 2;
|
||
}
|
||
}
|
||
}
|
||
|
||
std::vector<std::vector<int>> zVertivalFlags_h;
|
||
std::vector<std::vector<SSG_RUN>> rimFeatures_h;
|
||
for (int line = 0; line < linePtNum; line++)
|
||
{
|
||
if (line == 1177)
|
||
int kkk = 1;
|
||
|
||
//根据z连续性分段
|
||
std::vector<SSG_RUN> segs;
|
||
wd_lineDataSegment_dist_2(
|
||
scanLines_h[line],
|
||
segs,
|
||
maxDistTh,
|
||
minSegLen
|
||
);
|
||
|
||
std::vector<int> line_verticalFlags;
|
||
wd_getXYVertialFeature_perSeg_dirAngleMethod(
|
||
scanLines_h[line],
|
||
segs,
|
||
line,
|
||
removeVertialPara,
|
||
line_verticalFlags
|
||
);
|
||
zVertivalFlags_h.push_back(line_verticalFlags);
|
||
|
||
//提取边沿特征
|
||
std::vector<SSG_RUN> line_rimFeatures;
|
||
wd_getRimConvexFeature(
|
||
scanLines_h[line],
|
||
segs,
|
||
line,
|
||
rimPara_1,
|
||
rimPara_2,
|
||
rimWidth,
|
||
line_rimFeatures
|
||
);
|
||
rimFeatures_h.push_back(line_rimFeatures);
|
||
|
||
for (int i = 0; i < (int)line_verticalFlags.size(); i++)
|
||
{
|
||
if (line_verticalFlags[i] > 0)
|
||
flags[i][line] = 1;
|
||
}
|
||
for (int i = 0; i < (int)line_rimFeatures.size(); i++)
|
||
{
|
||
SSG_RUN& a_rim = line_rimFeatures[i];
|
||
for (int j = 0; j < a_rim.len; j++)
|
||
{
|
||
if (flags[j+a_rim.start][line] == 2)
|
||
flags[j + a_rim.start][line] = 4;
|
||
else
|
||
flags[j + a_rim.start][line] = 3;
|
||
}
|
||
}
|
||
}
|
||
//标注
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
for (int j = 0; j < linePtNum; j++)
|
||
{
|
||
if (flags[line][j] == 1)
|
||
{
|
||
scanLines[line][j].pt3D.z = 0;
|
||
scanLines_h[j][line].pt3D.z = 0;
|
||
}
|
||
}
|
||
}
|
||
//迭代一次
|
||
SSG_lineSegParam lineSegPara;
|
||
lineSegPara.distScale = 5.0;
|
||
lineSegPara.segGapTh_y = 5.0;
|
||
lineSegPara.segGapTh_z = 5.0;
|
||
int minSegSize = 1;
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
std::vector<SSG_RUN> segs;
|
||
wd_getLineDataIntervals(
|
||
scanLines[line],
|
||
lineSegPara,
|
||
segs);
|
||
for (int i = 0; i < (int)segs.size(); i++)
|
||
{
|
||
if (segs[i].len <= minSegSize)
|
||
{
|
||
int idx0 = segs[i].start;
|
||
for (int j = 0; j < segs[i].len; j++)
|
||
flags[line][idx0 + j] = 1;
|
||
}
|
||
}
|
||
}
|
||
for (int line = 0; line < linePtNum; line++)
|
||
{
|
||
std::vector<SSG_RUN> segs;
|
||
wd_getLineDataIntervals(
|
||
scanLines_h[line],
|
||
lineSegPara,
|
||
segs);
|
||
for (int i = 0; i < (int)segs.size(); i++)
|
||
{
|
||
if (segs[i].len <= minSegSize)
|
||
{
|
||
int idx0 = segs[i].start;
|
||
for (int j = 0; j < segs[i].len; j++)
|
||
flags[idx0 + j][line] = 1;
|
||
}
|
||
}
|
||
}
|
||
|
||
//标注
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
for (int j = 0; j < linePtNum; j++)
|
||
{
|
||
scanLinesInput[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
|
||
scanLines[line][j].nPointIdx = 0;
|
||
}
|
||
}
|
||
//将垂直线段去除
|
||
std::vector< SVzNL3DPosition> validPoints;
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
for (int j = 0; j < linePtNum; j++)
|
||
{
|
||
if (flags[line][j] == 1)
|
||
{
|
||
scanLinesInput[line][j].pt3D.z = 0;
|
||
scanLines[line][j].pt3D.z = 0;
|
||
}
|
||
else if (flags[line][j] > 0)
|
||
scanLinesInput[line][j].nPointIdx = flags[line][j];
|
||
|
||
if (scanLines[line][j].pt3D.z > 1e-4)
|
||
{
|
||
SVzNL3DPosition a_vldPt;
|
||
a_vldPt.pt3D = scanLines[line][j].pt3D;
|
||
a_vldPt.nPointIdx = (line << 16) | (j & 0xffff);
|
||
validPoints.push_back(a_vldPt);
|
||
}
|
||
}
|
||
}
|
||
|
||
//聚类
|
||
int clusterCheckWin = 5;
|
||
double clusterDist = 5.0;
|
||
int distType = 1; //0 - 2d distance; 1- 3d distance
|
||
std::vector<std::vector< SVzNL3DPosition>> objClusters; //result
|
||
wd_pointClustering_speedUp(
|
||
validPoints,
|
||
lineNum, linePtNum, clusterCheckWin, //搜索窗口
|
||
clusterDist,
|
||
distType,
|
||
objClusters //result
|
||
);
|
||
|
||
//保留最前面的目标, 筐子
|
||
double minObj_L = rotorParam.basket_L * 0.8;
|
||
double maxObj_L = rotorParam.basket_L * 1.25;
|
||
double minObj_W = rotorParam.basket_W * 0.8;
|
||
double maxObj_W = rotorParam.basket_W * 1.25;
|
||
const double topPlateMaxZRange = 100.0;
|
||
std::vector<double> objMeanZ;
|
||
std::vector<SSG_ROIRectD> objROIs;
|
||
std::vector<SVzNLRangeD> objZRange;
|
||
objMeanZ.resize(objClusters.size());
|
||
objROIs.resize(objClusters.size());
|
||
objZRange.resize(objClusters.size());
|
||
std::vector<int> validObjIndice;
|
||
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 len = w < h ? h : w;
|
||
double width = w > h ? h : w;
|
||
if ((len > minObj_L) && (width > minObj_W) && (len < maxObj_L) && (width < maxObj_W))
|
||
{
|
||
SVzNLRangeD zRange;
|
||
double meanZ = _getListMeanZ(objClusters[i], zRange);
|
||
objMeanZ[i] = meanZ;
|
||
objZRange[i] = zRange;
|
||
validObjIndice.push_back(i);
|
||
}
|
||
else
|
||
{
|
||
objMeanZ[i] = 0;
|
||
objZRange[i].max = -1.0;
|
||
objZRange[i].min = 0.0;
|
||
}
|
||
}
|
||
|
||
if (validObjIndice.size() == 0)
|
||
{
|
||
*errCode = SG_ERR_ZERO_OBJECTS;
|
||
return;
|
||
}
|
||
|
||
//最前面的目标为筐子
|
||
int basketClusterIdx = validObjIndice[0];
|
||
for (int i = 1; i < (int)validObjIndice.size(); i++)
|
||
{
|
||
int idx = validObjIndice[i];
|
||
if (objMeanZ[basketClusterIdx] > objMeanZ[idx])
|
||
basketClusterIdx = idx;
|
||
}
|
||
std::vector< SVzNL3DPosition>& basketCluster = objClusters[basketClusterIdx];
|
||
//提取筐子的四个角点，用于确定目标所在的边
|
||
|
||
//生成投影图，方便使用模板
|
||
const double quantiScale = 1.0; //
|
||
SSG_ROIRectD& basketROI = objROIs[basketClusterIdx];
|
||
int quantiW = (basketROI.right - basketROI.left) / quantiScale + 1;
|
||
if (quantiW % 2 == 1)
|
||
quantiW += 1;
|
||
int quantiH = (basketROI.bottom - basketROI.top) / quantiScale + 1;
|
||
if (quantiH % 2 == 1)
|
||
quantiH += 1;
|
||
|
||
std::vector<std::vector<double>> quantiData;
|
||
quantiData.resize(quantiW);
|
||
for (int i = 0; i < quantiW; i++)
|
||
{
|
||
quantiData[i].resize(quantiH);
|
||
std::fill(quantiData[i].begin(), quantiData[i].end(), -1);
|
||
}
|
||
//量化
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
for (int j = 0; j < linePtNum; j++)
|
||
{
|
||
if ((scanLines[line][j].pt3D.z > 1e-4) &&
|
||
(scanLines[line][j].pt3D.x >= basketROI.left) && (scanLines[line][j].pt3D.x < basketROI.right) &&
|
||
(scanLines[line][j].pt3D.y >= basketROI.top) && (scanLines[line][j].pt3D.y < basketROI.bottom))
|
||
{
|
||
int quantiX = (scanLines[line][j].pt3D.x - basketROI.left) / quantiScale;
|
||
int quantiY = (scanLines[line][j].pt3D.y - basketROI.top) / quantiScale;
|
||
quantiData[quantiX][quantiY] = scanLines[line][j].pt3D.z;
|
||
}
|
||
}
|
||
}
|
||
#ifdef _DEBUG_OUTPUT
|
||
//输出图像
|
||
cv::Mat vis = vec2Float2VisMat(quantiData);
|
||
cv::imwrite("quanti_visual.png", vis);
|
||
#endif
|
||
|
||
//生成模板
|
||
double template_minR = rotorParam.rotorDiameter/2.0 - 1.0;
|
||
double template_maxR = rotorParam.rotorDiameter/2.0 + 1.0;
|
||
double template_centerR = 10.0;
|
||
int template_pixR = (int)(template_maxR / quantiScale + 1);
|
||
int template_W = template_pixR * 2 + 1;
|
||
|
||
double template_cx = (double)template_pixR * quantiScale;
|
||
double template_cy = template_cx;
|
||
std::vector<std::vector<int>> templateMask;
|
||
templateMask.resize(template_W);
|
||
for (int i = 0; i < template_W; i++)
|
||
templateMask[i].resize(template_W);
|
||
for (int i = 0; i < template_W; i++)
|
||
{
|
||
for (int j = 0; j < template_W; j++)
|
||
{
|
||
double x = (double)i * quantiScale;
|
||
double y = (double)j * quantiScale;
|
||
double dist = sqrt(pow(x - template_cx, 2) + pow(y - template_cy, 2));
|
||
if ((dist >= template_minR) && (dist <= template_maxR))
|
||
templateMask[i][j] = 1;
|
||
else if (dist <= template_centerR)
|
||
templateMask[i][j] = 2;
|
||
}
|
||
}
|
||
if ((template_W >= quantiW) || (template_W >= quantiH))
|
||
{
|
||
*errCode = SG_ERR_ZERO_OBJECTS;
|
||
return;
|
||
}
|
||
#ifdef _DEBUG_OUTPUT
|
||
//输出图像
|
||
cv::Mat vis_template = vec2Int2VisMat(templateMask);
|
||
cv::imwrite("template_visual.png", vis_template);
|
||
#endif
|
||
//搜索目标
|
||
std::vector<std::vector<SSG_meanVar>> statsCircle;
|
||
std::vector<std::vector<SSG_meanVar>> statsCenter;
|
||
statsCircle.resize(quantiW);
|
||
statsCenter.resize(quantiW);
|
||
for (int i = 0; i < quantiW; i++)
|
||
{
|
||
statsCircle[i].resize(quantiH);
|
||
statsCenter[i].resize(quantiH);
|
||
}
|
||
|
||
int searchStart_X = template_pixR;
|
||
int searchEnd_X = quantiW - 1 - template_pixR;
|
||
int searchStart_Y = template_pixR;
|
||
int searchEnd_Y = quantiH - 1 - template_pixR;
|
||
#if 0
|
||
for (int x = searchStart_X; x < searchEnd_X; x++)
|
||
{
|
||
for (int y = searchStart_Y; y < searchEnd_Y; y++)
|
||
{
|
||
if ((x == 373) && (y == 72))
|
||
int kkk = 1;
|
||
std::vector<double> circleZ;
|
||
std::vector<double> centerZ;
|
||
//模板检查
|
||
_templateChecking(
|
||
quantiData,
|
||
x - template_pixR, y - template_pixR,
|
||
templateMask,
|
||
circleZ,
|
||
centerZ);
|
||
//计算均值和方差
|
||
SSG_meanVar satasInfo_circle = { 0.0, 0.0 };
|
||
CalcStats(circleZ, satasInfo_circle);
|
||
statsCircle[x][y] = satasInfo_circle;
|
||
|
||
SSG_meanVar satasInfo_center = { 0.0, 0.0 };
|
||
CalcStats(centerZ, satasInfo_center);
|
||
statsCenter[x][y] = satasInfo_center;
|
||
}
|
||
}
|
||
#endif
|
||
#ifdef _DEBUG_OUTPUT
|
||
//输出图像
|
||
cv::Mat vis_circle = vec2Var2VisMat(statsCircle);
|
||
cv::imwrite("var_circle_visual.png", vis_circle);
|
||
cv::Mat vis_center = vec2Var2VisMat(statsCenter);
|
||
cv::imwrite("var_center_visual.png", vis_center);
|
||
cv::Mat vis_circleCenter = vec2VarAdd2VisMat(statsCircle, statsCenter);
|
||
cv::imwrite("var_center_circle_visual.png", vis_circleCenter);
|
||
#endif
|
||
|
||
//搜索方差极小值点, circle和center的方差累加后的极小值
|
||
double rotorH_th = rotorParam.rotorH * 0.8;
|
||
//两步搜索法：先搜索5x5的，再搜索模块大小
|
||
std::vector<std::vector< SSG_2DValueI>> varPeaks_5x5; //按行存储
|
||
varPeaks_5x5.resize(quantiW);
|
||
const SVzNL2DPoint searchIndice[24] = {
|
||
{-2,-2}, {-1, -2}, {0, -2}, { 1, -2 }, {2, -2 },
|
||
{-2,-1}, {-1, -1}, {0, -1}, { 1, -1 }, {2, -1 },
|
||
{-2, 0}, {-1, 0}, {1, 0}, {2, 0 },
|
||
{-2, 1}, {-1, 1}, {0, 1}, { 1, 1 }, {2, 1 },
|
||
{-2, 2}, {-1, 2}, {0, 2}, { 1, 2 }, {2, 2 }
|
||
};
|
||
//使用一个辅助Flag来加速
|
||
std::vector<std::vector<int>> peakFalseflags;
|
||
peakFalseflags.resize(quantiW);
|
||
for (int i = 0; i < quantiW; i++)
|
||
peakFalseflags[i].resize(quantiH);
|
||
for (int x = searchStart_X; x < searchEnd_X; x++)
|
||
{
|
||
for (int y = searchStart_Y; y < searchEnd_Y; y++)
|
||
{
|
||
if ((x == 373) && (y == 70))
|
||
int kkk = 1;
|
||
SSG_meanVar& curr_circle = statsCircle[x][y];
|
||
SSG_meanVar& curr_center = statsCenter[x][y];
|
||
if (peakFalseflags[x][y] > 0)
|
||
continue;
|
||
|
||
if ((curr_center.mean - curr_circle.mean) > rotorH_th)
|
||
{
|
||
bool isPeak = true;
|
||
for (int m = 0; m < 24; m++)
|
||
{
|
||
const SVzNL2DPoint& an_idx = searchIndice[m];
|
||
SSG_meanVar& chk_circle = statsCircle[x + an_idx.x][y+an_idx.y];
|
||
if (curr_circle.var > chk_circle.var)
|
||
{
|
||
isPeak = false;
|
||
break;
|
||
}
|
||
}
|
||
if (true == isPeak)
|
||
{
|
||
for (int m = 0; m < 24; m++)
|
||
{
|
||
const SVzNL2DPoint& an_idx = searchIndice[m];
|
||
peakFalseflags[x + an_idx.x][y + an_idx.y] = 1;
|
||
}
|
||
SSG_2DValueI a_peak;
|
||
a_peak.x = x;
|
||
a_peak.y = y;
|
||
a_peak.value = 0;
|
||
a_peak.valueD = curr_circle.var;
|
||
a_peak.sideID = 0; //1-T, 2-B, 3-L, 4-R
|
||
varPeaks_5x5[x].push_back(a_peak);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
//二次搜索
|
||
std::vector< SSG_2DValueI> objVarPeaks;
|
||
int searchRng = (int)( rotorParam.rotorDiameter / quantiScale + 0.5 );
|
||
for (int x = searchStart_X; x < searchEnd_X; x++)
|
||
{
|
||
int pkSize = (int)varPeaks_5x5[x].size();
|
||
if (pkSize > 0)
|
||
{
|
||
for (int m = 0; m < pkSize; m++)
|
||
{
|
||
SSG_2DValueI& a_peak = varPeaks_5x5[x][m];
|
||
if (a_peak.value == 1)
|
||
continue;
|
||
|
||
bool isPeak = validateVarPeak(varPeaks_5x5, a_peak, searchRng);
|
||
if (true == isPeak)
|
||
{
|
||
objVarPeaks.push_back(varPeaks_5x5[x][m]);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
#ifdef _DEBUG_OUTPUT
|
||
//输出图像
|
||
cv::Mat vis_gray = vec2Float2VisMat(quantiData);
|
||
cv::Mat vis_color;
|
||
cv::cvtColor(vis_gray, vis_color, cv::COLOR_GRAY2BGR);
|
||
int radius = 3;
|
||
cv::Scalar color_all(0, 255, 0); // 颜色 (BGR顺序：蓝色, 绿色, 红色)
|
||
for (int x = searchStart_X; x < searchEnd_X; x++)
|
||
{
|
||
int pkSize = (int)varPeaks_5x5[x].size();
|
||
if (pkSize > 0)
|
||
{
|
||
for (int m = 0; m < pkSize; m++)
|
||
{
|
||
int px = varPeaks_5x5[x][m].x;
|
||
int py = varPeaks_5x5[x][m].y;
|
||
cv::circle(vis_color, cv::Point(py, px), radius, color_all, -1);
|
||
}
|
||
}
|
||
}
|
||
cv::Scalar color(0, 0, 255); // 颜色 (BGR顺序：蓝色, 绿色, 红色)
|
||
for (int i = 0; i < (int)objVarPeaks.size(); i++)
|
||
{
|
||
int px = objVarPeaks[i].x;
|
||
int py = objVarPeaks[i].y;
|
||
// thickness = -1 表示实心
|
||
cv::circle(vis_color, cv::Point(py, px), radius, color, -1);
|
||
}
|
||
|
||
cv::imwrite("quanti_obj_color.png", vis_color);
|
||
#endif
|
||
|
||
//旋转回原坐标系
|
||
|
||
for (int i = 0; i < (int)rotorPositions.size(); i++)
|
||
{
|
||
rotorPositions[i].center = _ptRotate(rotorPositions[i].center, groundCalibPara.invRMatrix);
|
||
rotorPositions[i].normDir = _ptRotate(rotorPositions[i].normDir, groundCalibPara.invRMatrix);
|
||
}
|
||
|
||
return;
|
||
}
|
||
|
||
//北京玖瑞工件定位
|
||
void Jiurui_getWorkpiecePose(
|
||
std::vector< std::vector<SVzNL3DPosition>>& scanLinesInput,
|
||
const SSG_cornerParam cornerPara,
|
||
const SSG_planeCalibPara groundCalibPara,
|
||
const WD_JiuruiWorkpieceParam workpieceParam,
|
||
std::vector<SSG_pointPose>& workpiecePositions,
|
||
int* errCode)
|
||
{
|
||
*errCode = 0;
|
||
|
||
//生成数据副本，使用副本数据进行调平和后续处理
|
||
int lineNum = (int)scanLinesInput.size();
|
||
std::vector< std::vector<SVzNL3DPosition>> scanLines;
|
||
scanLines.resize(lineNum);
|
||
int linePtNum = (int)scanLinesInput[0].size();
|
||
bool isGridData = true;
|
||
for (int i = 0; i < lineNum; i++)
|
||
{
|
||
if (linePtNum != (int)scanLinesInput[i].size())
|
||
isGridData = false;
|
||
|
||
scanLines[i].resize(scanLinesInput[i].size());
|
||
std::copy(scanLinesInput[i].begin(), scanLinesInput[i].end(), scanLines[i].begin()); // 使用std::copy算法
|
||
|
||
for (int j = 0; j < (int)scanLinesInput[i].size(); j++)
|
||
scanLinesInput[i][j].nPointIdx = 0; //清零，用于debug时记录信息
|
||
}
|
||
if (false == isGridData)//数据不是网格格式
|
||
{
|
||
*errCode = SG_ERR_NOT_GRID_FORMAT;
|
||
return;
|
||
}
|
||
|
||
double removeGroundHeight = -1;
|
||
for (int i = 0; i < lineNum; i++)
|
||
{ //行处理
|
||
//调平，去除地面
|
||
wd_lineDataR(scanLines[i], groundCalibPara.planeCalib, removeGroundHeight);
|
||
}
|
||
|
||
//产生水平扫描数据
|
||
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;
|
||
}
|
||
|
||
//算法流程：
|
||
//1、检查垂直方向数据并去除
|
||
//2、聚类
|
||
//3、保留最前面目标
|
||
//4、提取孔
|
||
//5、拟合
|
||
//6、计算中间坐标
|
||
//内部参数
|
||
SSG_cornerParam removeVertialPara = cornerPara;
|
||
removeVertialPara.scale = 10.0;
|
||
removeVertialPara.cornerTh = 60;
|
||
|
||
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<std::vector<int>> zVertivalFlags;
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
if (line == 700)
|
||
int kkk = 1;
|
||
std::vector<int> line_verticalFlags;
|
||
wd_getXYVertialFeature_dirAngleMethod(
|
||
scanLines[line],
|
||
line,
|
||
removeVertialPara,
|
||
line_verticalFlags
|
||
);
|
||
zVertivalFlags.push_back(line_verticalFlags);
|
||
|
||
for (int i = 0; i < (int)line_verticalFlags.size(); i++)
|
||
{
|
||
if (line_verticalFlags[i] > 0)
|
||
flags[line][i] = 1;
|
||
}
|
||
}
|
||
|
||
std::vector<std::vector<int>> zVertivalFlags_h;
|
||
for (int line = 0; line < linePtNum; line++)
|
||
{
|
||
if (line == 1177)
|
||
int kkk = 1;
|
||
std::vector<int> line_verticalFlags;
|
||
wd_getXYVertialFeature_dirAngleMethod(
|
||
scanLines_h[line],
|
||
line,
|
||
removeVertialPara,
|
||
line_verticalFlags
|
||
);
|
||
zVertivalFlags_h.push_back(line_verticalFlags);
|
||
|
||
for (int i = 0; i < (int)line_verticalFlags.size(); i++)
|
||
{
|
||
if (line_verticalFlags[i] > 0)
|
||
flags[i][line] = 1;
|
||
}
|
||
}
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
for (int j = 0; j < linePtNum; j++)
|
||
{
|
||
if (flags[line][j] > 0)
|
||
{
|
||
scanLines[line][j].pt3D.z = 0;
|
||
scanLines_h[j][line].pt3D.z = 0;
|
||
}
|
||
}
|
||
}
|
||
//迭代一次
|
||
SSG_lineSegParam lineSegPara;
|
||
lineSegPara.distScale = 5.0;
|
||
lineSegPara.segGapTh_y = 5.0;
|
||
lineSegPara.segGapTh_z = 5.0;
|
||
const int minSegLen = 5;
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
std::vector<SSG_RUN> segs;
|
||
wd_getLineDataIntervals(
|
||
scanLines[line],
|
||
lineSegPara,
|
||
segs);
|
||
for (int i = 0; i < (int)segs.size(); i++)
|
||
{
|
||
if (segs[i].len <= minSegLen)
|
||
{
|
||
int idx0 = segs[i].start;
|
||
for (int j = 0; j < segs[i].len; j++)
|
||
flags[line][idx0 + j] = 1;
|
||
}
|
||
}
|
||
}
|
||
for (int line = 0; line < linePtNum; line++)
|
||
{
|
||
std::vector<SSG_RUN> segs;
|
||
wd_getLineDataIntervals(
|
||
scanLines_h[line],
|
||
lineSegPara,
|
||
segs);
|
||
for (int i = 0; i < (int)segs.size(); i++)
|
||
{
|
||
if (segs[i].len <= minSegLen)
|
||
{
|
||
int idx0 = segs[i].start;
|
||
for (int j = 0; j < segs[i].len; j++)
|
||
flags[idx0 + j][line] = 1;
|
||
}
|
||
}
|
||
}
|
||
|
||
//标注
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
for (int j = 0; j < linePtNum; j++)
|
||
{
|
||
scanLinesInput[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
|
||
scanLines[line][j].nPointIdx = 0;
|
||
}
|
||
}
|
||
//将垂直线段去除
|
||
std::vector< SVzNL3DPosition> validPoints;
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
for (int j = 0; j < linePtNum; j++)
|
||
{
|
||
if (flags[line][j] > 0)
|
||
scanLines[line][j].pt3D.z = 0;
|
||
|
||
if (scanLines[line][j].pt3D.z > 1e-4)
|
||
{
|
||
SVzNL3DPosition a_vldPt;
|
||
a_vldPt.pt3D = scanLines[line][j].pt3D;
|
||
a_vldPt.nPointIdx = (line << 16) | (j & 0xffff);
|
||
validPoints.push_back(a_vldPt);
|
||
}
|
||
}
|
||
}
|
||
|
||
//聚类
|
||
int clusterCheckWin = 5;
|
||
double clusterDist = 5.0;
|
||
int distType = 1; //0 - 2d distance; 1- 3d distance
|
||
std::vector<std::vector< SVzNL3DPosition>> objClusters; //result
|
||
wd_pointClustering_speedUp(
|
||
validPoints,
|
||
lineNum, linePtNum, clusterCheckWin, //搜索窗口
|
||
clusterDist,
|
||
distType,
|
||
objClusters //result
|
||
);
|
||
|
||
//保留最前面的大小合适的目标
|
||
double minObjSize = workpieceParam.width * 0.8;
|
||
double maxObjSize = workpieceParam.len * 1.5;
|
||
const double topPlateMaxZRange = 100.0;
|
||
std::vector<double> objMeanZ;
|
||
std::vector<SSG_ROIRectD> objROIs;
|
||
std::vector<SVzNLRangeD> objZRange;
|
||
objMeanZ.resize(objClusters.size());
|
||
objROIs.resize(objClusters.size());
|
||
objZRange.resize(objClusters.size());
|
||
std::vector<int> validObjIndice;
|
||
int topClusterId = -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;
|
||
if ((w > minObjSize) && (h > minObjSize) && (w < maxObjSize) && (h < maxObjSize))
|
||
{
|
||
SVzNLRangeD zRange;
|
||
double meanZ = _getListMeanZ(objClusters[i], zRange);
|
||
objMeanZ[i] = meanZ;
|
||
objZRange[i] = zRange;
|
||
validObjIndice.push_back(i);
|
||
|
||
if (topClusterId < 0)
|
||
topClusterId = i;
|
||
else if (objMeanZ[topClusterId] > meanZ)
|
||
topClusterId = i;
|
||
}
|
||
else
|
||
{
|
||
objMeanZ[i] = 0;
|
||
objZRange[i].max = -1.0;
|
||
objZRange[i].min = 0.0;
|
||
}
|
||
}
|
||
|
||
if ( (validObjIndice.size() == 0) || (topClusterId < 0))
|
||
{
|
||
*errCode = SG_ERR_ZERO_OBJECTS;
|
||
return;
|
||
}
|
||
|
||
//标注
|
||
//重新将flags设置为目标的mask
|
||
for (int i = 0; i < lineNum; i++)
|
||
std::fill(flags[i].begin(), flags[i].end(), -1);
|
||
|
||
for (int i = 0; i < (int)objClusters[topClusterId].size(); i++)
|
||
{
|
||
int line = objClusters[topClusterId][i].nPointIdx >> 16;
|
||
int ptIdx = objClusters[topClusterId][i].nPointIdx & 0x0000FFFF;
|
||
scanLinesInput[line][ptIdx].nPointIdx = 2;
|
||
flags[line][ptIdx] = 1; //indexing
|
||
}
|
||
|
||
//计算质心
|
||
SVzNL3DPoint centroid = { 0, 0, 0 };
|
||
int centroid_size = (int)objClusters[topClusterId].size();
|
||
for (int i = 0; i < centroid_size; i++)
|
||
{
|
||
int line = objClusters[topClusterId][i].nPointIdx >> 16;
|
||
int ptIdx = objClusters[topClusterId][i].nPointIdx & 0x0000FFFF;
|
||
centroid.x += scanLinesInput[line][ptIdx].pt3D.x;
|
||
centroid.y += scanLinesInput[line][ptIdx].pt3D.y;
|
||
centroid.z += scanLinesInput[line][ptIdx].pt3D.z;
|
||
}
|
||
centroid.x = centroid.x / centroid_size;
|
||
centroid.y = centroid.y / centroid_size;
|
||
centroid.z = centroid.z / centroid_size;
|
||
|
||
//圆周扫描:0.5度间隔
|
||
double angleScale = 0.5;
|
||
std::vector<std::vector<SWD_polarPt>> polarScanData;
|
||
int polarLines = (int)(360.0 / angleScale + 0.5);
|
||
polarScanData.resize(polarLines);
|
||
_computeAnlgeScanData(
|
||
objClusters[topClusterId],
|
||
angleScale,
|
||
centroid,
|
||
polarScanData);
|
||
|
||
//每个扫描角度按R从小到大排序
|
||
for (int i = 0; i < polarLines; i++)
|
||
{
|
||
std::sort(polarScanData[i].begin(), polarScanData[i].end(), compareByPolarScanR);
|
||
}
|
||
|
||
//提取平面点-计算法向
|
||
double distToPlane_min = 17.5;
|
||
double distToPlane_max = 18.5;
|
||
std::vector<SWD_polarPt> planePolarPoints;
|
||
std::vector<cv::Point3d> Points3ds;
|
||
for (int i = 0; i < polarLines; i++)
|
||
{
|
||
int lineSize = (int)polarScanData[i].size();
|
||
if (lineSize == 0)
|
||
continue;
|
||
|
||
int minR = polarScanData[i][0].R;
|
||
for (int j = 0; j < lineSize; j++)
|
||
{
|
||
double dist = polarScanData[i][j].R - minR;
|
||
if ((dist >= distToPlane_min) && (dist <= distToPlane_max))
|
||
{
|
||
planePolarPoints.push_back(polarScanData[i][j]);
|
||
cv::Point3d a_pt = cv::Point3d(polarScanData[i][j].x, polarScanData[i][j].y, polarScanData[i][j].z);
|
||
Points3ds.push_back(a_pt);
|
||
}
|
||
}
|
||
}
|
||
//标注
|
||
for (int i = 0; i < (int)planePolarPoints.size(); i++)
|
||
{
|
||
int line = planePolarPoints[i].lineIdx;
|
||
int ptIdx = planePolarPoints[i].ptIdx;
|
||
scanLinesInput[line][ptIdx].nPointIdx = 3;
|
||
}
|
||
//拟合
|
||
//计算面参数: z = Ax + By + C
|
||
//res: [0]=A, [1]= B, [2]=-1.0, [3]=C,
|
||
std::vector<double> res;
|
||
vzCaculateLaserPlane(Points3ds, res);
|
||
|
||
//计算投影向量
|
||
SVzNL3DPoint vec_1 = { res[0], res[1], res[2]};
|
||
SVzNL3DPoint vec_z = { 0, 0, 1.0 };
|
||
SSG_planeCalibPara poseR = wd_computeRTMatrix(vec_1, vec_z);
|
||
|
||
//投影
|
||
std::vector<SVzNL3DPosition> projectPosition3ds;
|
||
for (int i = 0; i < (int)objClusters[topClusterId].size(); i++)
|
||
{
|
||
SVzNL3DPosition a_vldPt;
|
||
a_vldPt.nPointIdx = objClusters[topClusterId][i].nPointIdx;
|
||
a_vldPt.pt3D = wd_ptRotate(objClusters[topClusterId][i].pt3D, poseR.planeCalib);
|
||
projectPosition3ds.push_back(a_vldPt);
|
||
}
|
||
|
||
//
|
||
//迭代：圆周扫描-提取轮廓-计算中心点和轴向
|
||
//
|
||
//迭代计算质心
|
||
centroid = { 0, 0, 0 };
|
||
for (int i = 0; i < centroid_size; i++)
|
||
{
|
||
int line = objClusters[topClusterId][i].nPointIdx >> 16;
|
||
int ptIdx = objClusters[topClusterId][i].nPointIdx & 0x0000FFFF;
|
||
centroid.x += scanLinesInput[line][ptIdx].pt3D.x;
|
||
centroid.y += scanLinesInput[line][ptIdx].pt3D.y;
|
||
centroid.z += scanLinesInput[line][ptIdx].pt3D.z;
|
||
}
|
||
centroid.x = centroid.x / centroid_size;
|
||
centroid.y = centroid.y / centroid_size;
|
||
centroid.z = centroid.z / centroid_size;
|
||
|
||
//圆周扫描:0.5度间隔
|
||
polarScanData.clear();
|
||
polarScanData.resize(polarLines);
|
||
_computeAnlgeScanData(
|
||
projectPosition3ds,
|
||
angleScale,
|
||
centroid,
|
||
polarScanData);
|
||
//每个扫描角度按R从小到大排序
|
||
for (int i = 0; i < polarLines; i++)
|
||
{
|
||
std::sort(polarScanData[i].begin(), polarScanData[i].end(), compareByPolarScanR);
|
||
}
|
||
|
||
//取轮廓点计算中心
|
||
SVzNL3DPoint objCenter = { 0, 0, 0 };
|
||
int centerSum = 0;
|
||
for (int i = 0; i < polarLines; i++)
|
||
{
|
||
int lineSize = (int)polarScanData[i].size();
|
||
if (lineSize == 0)
|
||
continue;
|
||
|
||
objCenter.x += polarScanData[i][0].x;
|
||
objCenter.y += polarScanData[i][0].y;
|
||
objCenter.z += polarScanData[i][0].z;
|
||
centerSum++;
|
||
}
|
||
objCenter.x = objCenter.x / centerSum;
|
||
objCenter.y = objCenter.y / centerSum;
|
||
objCenter.z = objCenter.z / centerSum;
|
||
|
||
//计算长轴方向
|
||
int degree180 = (int)(180.0 / angleScale);
|
||
int longAxisIndex = -1;
|
||
double longAxisLen = -1;
|
||
for (int i = 0; i < polarLines/2; i++)
|
||
{
|
||
int idx_1 = i;
|
||
int idx_2 = i + degree180;
|
||
if (idx_2 < polarLines)
|
||
{
|
||
if ((polarScanData[idx_1].size() == 0) || (polarScanData[idx_2].size() == 0))
|
||
{
|
||
double sumR = polarScanData[idx_1][0].R + polarScanData[idx_2][0].R;
|
||
if (longAxisIndex < 0)
|
||
{
|
||
longAxisIndex = idx_1;
|
||
longAxisLen = sumR;
|
||
}
|
||
else
|
||
{
|
||
if (longAxisLen < sumR)
|
||
{
|
||
longAxisIndex = idx_1;
|
||
longAxisLen = sumR;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
if(longAxisIndex < 0)
|
||
{
|
||
*errCode = SG_ERR_ZERO_OBJECTS;
|
||
return;
|
||
}
|
||
|
||
double longAxisAngle = longAxisIndex * angleScale;
|
||
SVzNL3DPoint vector_y = {cos(longAxisAngle/PI), sin(longAxisAngle / PI), 0 };
|
||
//叉乘出vector_x
|
||
SVzNL3DPoint vector_x;
|
||
vector_x.x = vector_y.y * vec_z.z - vec_z.y * vector_y.z;
|
||
vector_x.y = vector_y.z * vec_z.x - vec_z.z * vector_y.x;
|
||
vector_x.z = vector_y.x * vec_z.y - vec_z.x * vector_y.y;
|
||
//生成结果
|
||
SSG_pointPose a_pose;
|
||
a_pose.point = _ptRotate(objCenter, poseR.invRMatrix);
|
||
a_pose.pose_x = _ptRotate(vector_x, poseR.invRMatrix);
|
||
a_pose.pose_y = _ptRotate(vector_y, poseR.invRMatrix);
|
||
a_pose.pose_z = _ptRotate(vec_z, poseR.invRMatrix);
|
||
|
||
//旋转回原坐标系
|
||
a_pose.point = _ptRotate(a_pose.point, groundCalibPara.invRMatrix);
|
||
a_pose.pose_x = _ptRotate(a_pose.pose_x, groundCalibPara.invRMatrix);
|
||
a_pose.pose_y = _ptRotate(a_pose.pose_y, groundCalibPara.invRMatrix);
|
||
a_pose.pose_z = _ptRotate(a_pose.pose_z, groundCalibPara.invRMatrix);
|
||
workpiecePositions.push_back(a_pose);
|
||
return;
|
||
}
|