wheelArchHeigthMeasure version 1.3.2 :
针对不理想点云改进了算法，增强鲁棒性
This commit is contained in:
parent
faada74612
commit
4d4c29893f
@ -39,6 +39,13 @@ typedef struct
|
||||
SWD3DPoint point;
|
||||
}SWDIndexing3DPoint;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int lineIdx;
|
||||
int ptIdx;
|
||||
SVzNL3DPoint point;
|
||||
}SWDIndexingVzPoint;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int start;
|
||||
@ -604,6 +611,7 @@ typedef struct
|
||||
double backwardDiffZ;
|
||||
double pre_stepDist;
|
||||
double post_stepDist;
|
||||
double curr_z;
|
||||
double forward_z;
|
||||
double backward_z;
|
||||
}SSG_pntDirAngle;
|
||||
|
||||
@ -32,6 +32,10 @@
|
||||
|
||||
//汽车轮眉高度测量
|
||||
#define SX_ERR_INVALID_ARC -2301
|
||||
#define SX_ERR_INVALID_WHEEL_EDGE -2302
|
||||
#define SX_ERR_NO_WHEEL_COMMON_EDGE -2303
|
||||
#define SX_ERR_NO_WHEEL_ARC -2304
|
||||
|
||||
|
||||
//糖袋子拆线
|
||||
#define SX_ERR_NO_MARK -2401
|
||||
|
||||
@ -3979,6 +3979,9 @@ void wd_computeDirAngle_wholeLine(
|
||||
int dataSize = (int)line_data.size();
|
||||
for (int i = 0; i < dataSize; i++)
|
||||
{
|
||||
if (i == 930)
|
||||
int kkk = 1;
|
||||
|
||||
if (line_data[i].pt3D.z < 1e-4)
|
||||
{
|
||||
ptDirAngles[i].pntIdx = i;
|
||||
@ -3988,6 +3991,7 @@ void wd_computeDirAngle_wholeLine(
|
||||
ptDirAngles[i].forwardAngle = 0;
|
||||
ptDirAngles[i].backwardAngle = 0;
|
||||
ptDirAngles[i].corner = 0;
|
||||
ptDirAngles[i].curr_z = 0;
|
||||
ptDirAngles[i].forwardDiffZ = 0;
|
||||
ptDirAngles[i].backwardDiffZ = 0;
|
||||
continue;
|
||||
@ -3995,14 +3999,15 @@ void wd_computeDirAngle_wholeLine(
|
||||
}
|
||||
//前向寻找
|
||||
int pre_i = -1;
|
||||
double pre_dist = 0;
|
||||
for (int j = i - 1; j >= 0; j--)
|
||||
{
|
||||
if (line_data[j].pt3D.z < 1e-4)
|
||||
continue;
|
||||
|
||||
double dist = sqrt(pow(line_data[i].pt3D.y - line_data[j].pt3D.y, 2) +
|
||||
pre_dist = sqrt(pow(line_data[i].pt3D.y - line_data[j].pt3D.y, 2) +
|
||||
pow(line_data[i].pt3D.z - line_data[j].pt3D.z, 2));
|
||||
if (dist >= cornerPara.scale)
|
||||
if (pre_dist >= cornerPara.scale)
|
||||
{
|
||||
pre_i = j;
|
||||
break;
|
||||
@ -4010,11 +4015,14 @@ void wd_computeDirAngle_wholeLine(
|
||||
}
|
||||
//后向寻找
|
||||
int post_i = -1;
|
||||
double post_dist = 0;
|
||||
for (int j = i + 1; j < dataSize; j++)
|
||||
{
|
||||
double dist = sqrt(pow(line_data[i].pt3D.y - line_data[j].pt3D.y, 2) +
|
||||
if (line_data[j].pt3D.z < 1e-4)
|
||||
continue;
|
||||
post_dist = sqrt(pow(line_data[i].pt3D.y - line_data[j].pt3D.y, 2) +
|
||||
pow(line_data[i].pt3D.z - line_data[j].pt3D.z, 2));
|
||||
if (dist >= cornerPara.scale)
|
||||
if (post_dist >= cornerPara.scale)
|
||||
{
|
||||
post_i = j;
|
||||
break;
|
||||
@ -4030,6 +4038,7 @@ void wd_computeDirAngle_wholeLine(
|
||||
ptDirAngles[i].forwardPntIdx = 0;
|
||||
ptDirAngles[i].backwardPntIdx = 0;
|
||||
ptDirAngles[i].corner = 0;
|
||||
ptDirAngles[i].curr_z = 0;
|
||||
ptDirAngles[i].forwardDiffZ = 0;
|
||||
ptDirAngles[i].backwardDiffZ = 0;
|
||||
}
|
||||
@ -4048,6 +4057,7 @@ void wd_computeDirAngle_wholeLine(
|
||||
ptDirAngles[i].corner = -(forwardAngle - backwardAngle); //图像坐标系与正常坐标系y方向相反，所以有“-”号
|
||||
ptDirAngles[i].forwardDiffZ = line_data[post_i].pt3D.z - line_data[i].pt3D.z;
|
||||
ptDirAngles[i].backwardDiffZ = line_data[i].pt3D.z - line_data[pre_i].pt3D.z;
|
||||
ptDirAngles[i].curr_z = line_data[i].pt3D.z;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -10,7 +10,8 @@
|
||||
//version 1.2.0 : 修正了地面调平，防止法向量旋转180度
|
||||
//version 1.3.0 : 修正了寻找上下点使用固定门限的问题
|
||||
//version 1.3.1 : 添加了一个检测轮毂是否存在的函数
|
||||
std::string m_strVersion = "1.3.1";
|
||||
//version 1.3.2 : 针对不理想点云改进了算法，增强鲁棒性
|
||||
std::string m_strVersion = "1.3.2";
|
||||
const char* wd_wheelArchHeigthMeasureVersion(void)
|
||||
{
|
||||
return m_strVersion.c_str();
|
||||
@ -273,6 +274,249 @@ bool wd_wheelPresenseDetection(
|
||||
|
||||
}
|
||||
|
||||
void _lineProc_getWheelEnding(
|
||||
std::vector< SVzNL3DPosition>& lineData,
|
||||
int lineIdx,
|
||||
std::vector<SWDIndexingVzPoint>& line_upEnding,
|
||||
std::vector<SWDIndexingVzPoint>& line_downEnding,
|
||||
double scale_segGap = 10.0,
|
||||
double scale_endingAngleLen = 25.0, //端点方向角计算尺度
|
||||
double wheel_endingAngleTh = 30)
|
||||
{
|
||||
int dataSize = (int)lineData.size();
|
||||
std::vector<SSG_RUN> segs;
|
||||
SSG_RUN a_run = { 0, -1, 0, }; //startIdx, len, lastIdx
|
||||
SVzNL3DPosition preData = { -1, {0, 0, 0} };
|
||||
for (int i = 0; i < dataSize; i++)
|
||||
{
|
||||
if ( (lineData[i].pt3D.z > 1e-4) && (lineData[i].nPointIdx == 1)) //wheel
|
||||
{
|
||||
if (a_run.len < 0)
|
||||
{
|
||||
a_run.start = i;
|
||||
a_run.len = 1;
|
||||
a_run.value = i;
|
||||
}
|
||||
else
|
||||
{
|
||||
double dist = sqrt(pow(lineData[i].pt3D.y - preData.pt3D.y, 2) + pow(lineData[i].pt3D.z - preData.pt3D.z, 2));
|
||||
if (dist < scale_segGap)
|
||||
{
|
||||
a_run.len = i - a_run.start + 1;
|
||||
a_run.value = i;
|
||||
}
|
||||
else
|
||||
{
|
||||
segs.push_back(a_run);
|
||||
|
||||
a_run.start = i;
|
||||
a_run.len = 1;
|
||||
a_run.value = i;
|
||||
}
|
||||
}
|
||||
preData = lineData[i];
|
||||
}
|
||||
}
|
||||
if (a_run.len > 0)
|
||||
segs.push_back(a_run);
|
||||
|
||||
//逐段计算端点方向角
|
||||
for (int i = 0; i < (int)segs.size(); i++)
|
||||
{
|
||||
SSG_RUN& a_seg = segs[i];
|
||||
int sPtIdx = a_seg.start;
|
||||
int ePtIdx = a_seg.value;
|
||||
//计算起点方向角
|
||||
for (int j = sPtIdx; j < ePtIdx; j++)
|
||||
{
|
||||
if (lineData[j].pt3D.z > 1e-4)
|
||||
{
|
||||
double dist = sqrt(pow(lineData[j].pt3D.y - lineData[sPtIdx].pt3D.y, 2) +
|
||||
pow(lineData[j].pt3D.z - lineData[sPtIdx].pt3D.z, 2));
|
||||
if (dist > scale_endingAngleLen)
|
||||
{
|
||||
double tanValue_post = (lineData[j].pt3D.z - lineData[sPtIdx].pt3D.z) / abs(lineData[j].pt3D.y - lineData[sPtIdx].pt3D.y);
|
||||
double forwardAngle = atan(tanValue_post) * 180.0 / PI;
|
||||
double corner = -(forwardAngle - 0);
|
||||
if (corner > wheel_endingAngleTh)
|
||||
{
|
||||
SWDIndexingVzPoint a_upEndig;
|
||||
a_upEndig.lineIdx = lineIdx;
|
||||
a_upEndig.ptIdx = sPtIdx;
|
||||
a_upEndig.point = lineData[sPtIdx].pt3D;
|
||||
line_upEnding.push_back(a_upEndig);
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
//计算终点方向角
|
||||
for (int j = ePtIdx; j > sPtIdx; j--)
|
||||
{
|
||||
if (lineData[j].pt3D.z > 1e-4)
|
||||
{
|
||||
double dist = sqrt(pow(lineData[ePtIdx].pt3D.y - lineData[j].pt3D.y, 2) +
|
||||
pow(lineData[ePtIdx].pt3D.z - lineData[j].pt3D.z, 2));
|
||||
if (dist > scale_endingAngleLen)
|
||||
{
|
||||
double tanValue_pre = (lineData[ePtIdx].pt3D.z - lineData[j].pt3D.z) / abs(lineData[ePtIdx].pt3D.y - lineData[j].pt3D.y);
|
||||
double backwardAngle = atan(tanValue_pre) * 180.0 / PI;
|
||||
double corner = -(0 - backwardAngle);
|
||||
if (corner > wheel_endingAngleTh)
|
||||
{
|
||||
SWDIndexingVzPoint a_downEndig;
|
||||
a_downEndig.lineIdx = lineIdx;
|
||||
a_downEndig.ptIdx = ePtIdx;
|
||||
a_downEndig.point = lineData[ePtIdx].pt3D;
|
||||
line_downEnding.push_back(a_downEndig);
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int treeState;
|
||||
int treeType;
|
||||
int sLineIdx;
|
||||
int eLineIdx;
|
||||
SSG_ROIRectD roi;
|
||||
std::vector< SWDIndexingVzPoint> treeNodes;
|
||||
}SSG_endingTree;
|
||||
|
||||
#define _TREE_STATE_UNDEF 0
|
||||
#define _TREE_STATE_ALIVE 1
|
||||
#define _TREE_STATE_DEAD 2
|
||||
//将feature在trees上寻找合适的生长点进行生长。如果没有合适的生长点， 返回false
|
||||
//没有使用全匹配。一个feature一旦被匹配上，匹配就完成。没有使用最佳匹配。
|
||||
bool _endingFeatureGrowing(SWDIndexingVzPoint& a_feature, std::vector<SSG_endingTree>& trees, SSG_treeGrowParam growParam)
|
||||
{
|
||||
for (int i = 0, i_max = (int)trees.size(); i < i_max; i++)
|
||||
{
|
||||
SSG_endingTree& a_tree = trees[i];
|
||||
if (_TREE_STATE_DEAD == a_tree.treeState)
|
||||
continue;
|
||||
//检查生长点
|
||||
SWDIndexingVzPoint last_node = a_tree.treeNodes.back();
|
||||
if (last_node.lineIdx == a_feature.lineIdx) //x为lineIdx，同一条扫描线上的不进行生长
|
||||
continue;
|
||||
|
||||
//判断生长点
|
||||
double y_diff = abs(a_feature.point.y - last_node.point.y);
|
||||
double z_diff = abs(a_feature.point.z - last_node.point.z);
|
||||
int line_diff = abs(a_feature.lineIdx - last_node.lineIdx);
|
||||
double x_diff = abs(a_feature.point.x - last_node.point.x);
|
||||
if ((y_diff < growParam.yDeviation_max) && (z_diff < growParam.zDeviation_max) &&
|
||||
((line_diff < growParam.maxLineSkipNum) || (x_diff < growParam.maxSkipDistance)))
|
||||
{
|
||||
a_tree.eLineIdx = a_feature.lineIdx;
|
||||
a_tree.treeNodes.push_back(a_feature);
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
void _getWheelEndingGrowingTrees(
|
||||
std::vector<std::vector<SWDIndexingVzPoint>>& endings,
|
||||
std::vector<SSG_endingTree>& trees,
|
||||
SSG_treeGrowParam growParam)
|
||||
{
|
||||
for (int i = 0, i_max = (int)endings.size(); i < i_max; i++)
|
||||
{
|
||||
std::vector<SWDIndexingVzPoint>& line_endings = endings[i];
|
||||
if (line_endings.size() > 0)
|
||||
{
|
||||
for (int j = 0, j_max = (int)line_endings.size(); j < j_max; j++)
|
||||
{
|
||||
SWDIndexingVzPoint& an_ending = line_endings[j];
|
||||
bool isMatched = _endingFeatureGrowing(an_ending, trees, growParam);
|
||||
if (false == isMatched)
|
||||
{
|
||||
//新的生长树
|
||||
SSG_endingTree a_newTree;
|
||||
a_newTree.treeNodes.push_back(an_ending);
|
||||
a_newTree.treeState = _TREE_STATE_ALIVE;
|
||||
a_newTree.treeType = 0;
|
||||
a_newTree.sLineIdx = an_ending.lineIdx;
|
||||
a_newTree.eLineIdx = an_ending.lineIdx;
|
||||
trees.push_back(a_newTree);
|
||||
}
|
||||
}
|
||||
}
|
||||
//检查停止生长的树,加速。
|
||||
//将生长节点为1的生长树移除
|
||||
int lineIdx = i;
|
||||
int m_max = (int)trees.size();
|
||||
for (int m = m_max - 1; m >= 0; m--) //从后往前，这样删除不会影响循环
|
||||
{
|
||||
if (_TREE_STATE_ALIVE == trees[m].treeState)
|
||||
{
|
||||
int line_diff = abs(lineIdx - trees[m].treeNodes.back().lineIdx);
|
||||
if (((growParam.maxLineSkipNum > 0) && (line_diff > growParam.maxLineSkipNum)) ||
|
||||
(i == i_max - 1))
|
||||
{
|
||||
trees[m].treeState = _TREE_STATE_DEAD;
|
||||
SWDIndexingVzPoint first_node = trees[m].treeNodes[0];
|
||||
SWDIndexingVzPoint last_node = trees[m].treeNodes.back();
|
||||
double len = sqrt(pow(first_node.point.x - last_node.point.x, 2) + pow(first_node.point.y - last_node.point.y, 2));
|
||||
if (len <= growParam.minVTypeTreeLen)
|
||||
trees.erase(trees.begin() + m);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
double _getTopY(SSG_endingTree& wheedEdge)
|
||||
{
|
||||
double minY = DBL_MAX;
|
||||
for (int i = 0; i < (int)wheedEdge.treeNodes.size(); i++)
|
||||
minY = minY > wheedEdge.treeNodes[i].point.y ? wheedEdge.treeNodes[i].point.y : minY;
|
||||
return minY;
|
||||
}
|
||||
|
||||
double _getBtmY(SSG_endingTree& wheedEdge)
|
||||
{
|
||||
double maxY = DBL_MIN;
|
||||
for (int i = 0; i < (int)wheedEdge.treeNodes.size(); i++)
|
||||
maxY = maxY < wheedEdge.treeNodes[i].point.y ? wheedEdge.treeNodes[i].point.y : maxY;
|
||||
return maxY;
|
||||
}
|
||||
|
||||
bool _compareByLineIdx(SSG_endingTree& a, SSG_endingTree& b)
|
||||
{
|
||||
return a.sLineIdx < b.sLineIdx;
|
||||
}
|
||||
|
||||
void _genEdgeContinuousPos(SSG_endingTree&edgeTree, std::vector<SVzNL2DPoint>& edgePos)
|
||||
{
|
||||
for (int i = 0; i < (int)edgeTree.treeNodes.size(); i++)
|
||||
{
|
||||
SVzNL2DPoint a_pos = { edgeTree.treeNodes[i].lineIdx, edgeTree.treeNodes[i].ptIdx };
|
||||
if (edgePos.size() == 0)
|
||||
edgePos.push_back(a_pos);
|
||||
else
|
||||
{
|
||||
SVzNL2DPoint last_pos = edgePos.back();
|
||||
if ((a_pos.x - last_pos.x) == 1)
|
||||
edgePos.push_back(a_pos);
|
||||
else //需要插值
|
||||
{
|
||||
for (int j = last_pos.x + 1; j < a_pos.x; j++)
|
||||
{
|
||||
int meany = (last_pos.y + a_pos.y) / 2;
|
||||
SVzNL2DPoint new_pos = {j, meany};
|
||||
edgePos.push_back(new_pos);
|
||||
}
|
||||
edgePos.push_back(a_pos);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//轮眉高度测量
|
||||
WD_wheelArchInfo wd_wheelArchHeigthMeasure(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||||
@ -284,6 +528,17 @@ WD_wheelArchInfo wd_wheelArchHeigthMeasure(
|
||||
int* errCode)
|
||||
{
|
||||
*errCode = 0;
|
||||
|
||||
//内部参数
|
||||
//使用尺度1确定轮胎上沿和下沿点
|
||||
double scale_segGap = 10.0;
|
||||
double scale_endingAngleLen = 25.0; //端点方向角计算尺度
|
||||
double wheel_endingAngleTh = 25;
|
||||
double arcScale_1 = 10.0;
|
||||
double arcScale_2 = 5.0;
|
||||
double computeLen = 150.0;
|
||||
int safeGuardLen = 20;
|
||||
|
||||
WD_wheelArchInfo result;
|
||||
memset(&result, 0, sizeof(WD_wheelArchInfo));
|
||||
|
||||
@ -417,7 +672,7 @@ WD_wheelArchInfo wd_wheelArchHeigthMeasure(
|
||||
{
|
||||
int lineIdx = cluster_wheel[i].x;
|
||||
int ptIdx = cluster_wheel[i].y;
|
||||
scanLines[lineIdx][ptIdx].nPointIdx = 1;
|
||||
scanLines[lineIdx][ptIdx].nPointIdx = 1; //wheel
|
||||
}
|
||||
for (int i = 0, i_max = (int)cluster_arc.size(); i < i_max; i++)
|
||||
{
|
||||
@ -426,41 +681,177 @@ WD_wheelArchInfo wd_wheelArchHeigthMeasure(
|
||||
scanLines[lineIdx][ptIdx].nPointIdx = 2;
|
||||
}
|
||||
|
||||
//寻找轮眉点
|
||||
//(1)快速寻找下端点（2）取中间区域，精确确定端点（3）抛物线拟合（4）计算轮眉点（最高点）
|
||||
std::vector<SVzNL2DPoint> arcEndings;
|
||||
_getArcEndings(scanLines, 2, arcEndings); //轮眉的ID是2
|
||||
if (arcEndings.size() == 0)
|
||||
std::vector<std::vector<SWDIndexingVzPoint>> upEndings;
|
||||
std::vector<std::vector<SWDIndexingVzPoint>> downEndings;
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
*errCode = SX_ERR_INVALID_ARC;
|
||||
if (line == 319)
|
||||
int kkk = 1;
|
||||
std::vector<SWDIndexingVzPoint> line_upEnding;
|
||||
std::vector<SWDIndexingVzPoint> line_downEnding;
|
||||
_lineProc_getWheelEnding(scanLines[line], line, line_upEnding, line_downEnding);
|
||||
upEndings.push_back(line_upEnding);
|
||||
downEndings.push_back(line_downEnding);
|
||||
}
|
||||
//生长聚类
|
||||
std::vector<SSG_endingTree> upEndingTrees;
|
||||
_getWheelEndingGrowingTrees(upEndings, upEndingTrees, growParam);
|
||||
|
||||
std::vector<SSG_endingTree> downEndingTrees;
|
||||
_getWheelEndingGrowingTrees(downEndings, downEndingTrees, growParam);
|
||||
|
||||
//确定轮胎上沿
|
||||
if ( (upEndingTrees.size() == 0) || (downEndingTrees.size() == 0))
|
||||
{
|
||||
*errCode = SX_ERR_INVALID_WHEEL_EDGE;
|
||||
return result;
|
||||
}
|
||||
int arcMidPos = _getArcEndingsCenterPos(scanLines, arcEndings);
|
||||
//取两侧固定长度（100mm内的点，进行精确边界提取，拟合）
|
||||
double chkWin = 100.0;
|
||||
std::vector< SVzNL2DPoint> arcFittingPos;
|
||||
_extractArcFittingPts(
|
||||
scanLines,
|
||||
arcEndings,
|
||||
chkWin,
|
||||
arcMidPos,
|
||||
arcFittingPos);
|
||||
//在XY平面生成拟合点
|
||||
double outLineLen = 200;
|
||||
SVzNL2DPoint arcPtPos;
|
||||
SVzNL3DPoint arcPt = _getWheelArcFittingPoint(scanLines, 3, arcFittingPos, arcPtPos);
|
||||
result.wheelArchPos = arcPt;
|
||||
result.arcLine[0] = { arcPt.x - outLineLen, arcPt.y, arcPt.z };
|
||||
result.arcLine[1] = { arcPt.x + outLineLen, arcPt.y, arcPt.z };
|
||||
|
||||
//以最上面的为轮子边沿
|
||||
SSG_endingTree& wheedEdge_up = upEndingTrees[0];
|
||||
if (upEndingTrees.size() > 1)
|
||||
{
|
||||
double minY = _getTopY(wheedEdge_up);
|
||||
for (int i = 1; i < (int)upEndingTrees.size(); i++)
|
||||
{
|
||||
double topY = _getTopY(upEndingTrees[i]);
|
||||
if (minY > topY)
|
||||
{
|
||||
wheedEdge_up = upEndingTrees[i];
|
||||
minY = topY;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (int i = 0; i < (int)downEndingTrees.size(); i++)
|
||||
{
|
||||
double btmY_0 = _getBtmY(downEndingTrees[i]);
|
||||
int left_0 = downEndingTrees[i].treeNodes[0].lineIdx;
|
||||
int right_0 = downEndingTrees[i].treeNodes.back().lineIdx;
|
||||
for (int j = i + 1; j < (int)downEndingTrees.size(); j++)
|
||||
{
|
||||
if (downEndingTrees[j].treeType < 0)
|
||||
continue;
|
||||
|
||||
double btmY_1 = _getBtmY(downEndingTrees[j]);
|
||||
int left_1 = downEndingTrees[j].treeNodes[0].lineIdx;
|
||||
int right_1 = downEndingTrees[j].treeNodes.back().lineIdx;
|
||||
if ((right_1 > left_0) && (right_0 > left_1))
|
||||
{
|
||||
if (btmY_0 < btmY_1)
|
||||
downEndingTrees[i].treeType = -1;
|
||||
else
|
||||
downEndingTrees[j].treeType = -1;
|
||||
}
|
||||
}
|
||||
}
|
||||
std::vector<SSG_endingTree> validDownEndingTrees;
|
||||
for (int i = 0; i < (int)downEndingTrees.size(); i++)
|
||||
{
|
||||
if (downEndingTrees[i].treeType >= 0)
|
||||
validDownEndingTrees.push_back(downEndingTrees[i]);
|
||||
}
|
||||
//排序
|
||||
std::sort(validDownEndingTrees.begin(), validDownEndingTrees.end(), _compareByLineIdx);
|
||||
std::vector<SVzNL2DPoint> downEdgePos;
|
||||
for (int i = 0; i < (int)validDownEndingTrees.size(); i++)
|
||||
_genEdgeContinuousPos(validDownEndingTrees[i], downEdgePos);
|
||||
|
||||
//生成每条扫描线的上沿。空的点进行插值得到
|
||||
std::vector<SVzNL2DPoint> upEdgePos;
|
||||
_genEdgeContinuousPos(wheedEdge_up, upEdgePos);
|
||||
|
||||
int start_up, start_down;
|
||||
if (upEdgePos[0].x < downEdgePos[0].x)
|
||||
{
|
||||
start_down = 0;
|
||||
start_up = downEdgePos[0].x - upEdgePos[0].x;
|
||||
}
|
||||
else
|
||||
{
|
||||
start_up = 0;
|
||||
start_down = upEdgePos[0].x - downEdgePos[0].x;
|
||||
}
|
||||
int commonNum = (int)upEdgePos.size() - start_up;
|
||||
int num2 = (int)downEdgePos.size() - start_down;
|
||||
if ((commonNum < 0) || (num2 < 0))
|
||||
{
|
||||
*errCode = SX_ERR_NO_WHEEL_COMMON_EDGE;
|
||||
return result;
|
||||
}
|
||||
|
||||
std::vector<SSG_intPair> all_edgePairs;
|
||||
commonNum = commonNum > num2 ? num2 : commonNum;
|
||||
for (int i = 0; i < commonNum; i++)
|
||||
{
|
||||
SSG_intPair a_pair;
|
||||
a_pair.idx = upEdgePos[i + start_up].x;
|
||||
a_pair.data_0 = upEdgePos[i + start_up].y;
|
||||
a_pair.data_1 = downEdgePos[i + start_down].y;
|
||||
all_edgePairs.push_back(a_pair);
|
||||
}
|
||||
|
||||
//提取轮胎上沿最小值，然后取左右100mm长范围内V型槽
|
||||
double wheelTopY = DBL_MAX;
|
||||
SSG_intPair wheelTopPos = { -1, -1, -1 };
|
||||
int topIdx = -1;
|
||||
for (int i = 0; i < (int)all_edgePairs.size(); i++)
|
||||
{
|
||||
if (scanLines[all_edgePairs[i].idx][all_edgePairs[i].data_0].pt3D.z < 1e-4)
|
||||
continue;
|
||||
|
||||
if (wheelTopY > scanLines[all_edgePairs[i].idx][all_edgePairs[i].data_0].pt3D.y)
|
||||
{
|
||||
wheelTopY = scanLines[all_edgePairs[i].idx][all_edgePairs[i].data_0].pt3D.y;
|
||||
wheelTopPos = all_edgePairs[i];
|
||||
topIdx = i;
|
||||
}
|
||||
}
|
||||
int startIdx = -1;
|
||||
for (int i = topIdx; i >= 0; i--)
|
||||
{
|
||||
if (scanLines[all_edgePairs[i].idx][all_edgePairs[i].data_0].pt3D.z < 1e-4)
|
||||
continue;
|
||||
|
||||
double dist = sqrt(pow(scanLines[wheelTopPos.idx][wheelTopPos.data_0].pt3D.x - scanLines[all_edgePairs[i].idx][all_edgePairs[i].data_0].pt3D.x, 2) +
|
||||
pow(scanLines[wheelTopPos.idx][wheelTopPos.data_0].pt3D.y - scanLines[all_edgePairs[i].idx][all_edgePairs[i].data_0].pt3D.y, 2));
|
||||
if (dist > computeLen)
|
||||
{
|
||||
startIdx = i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (startIdx < 0)
|
||||
startIdx = 0;
|
||||
|
||||
int endIdx = -1;
|
||||
for (int i = topIdx; i < (int)all_edgePairs.size(); i++)
|
||||
{
|
||||
if (scanLines[all_edgePairs[i].idx][all_edgePairs[i].data_0].pt3D.z < 1e-4)
|
||||
continue;
|
||||
|
||||
double dist = sqrt(pow(scanLines[wheelTopPos.idx][wheelTopPos.data_0].pt3D.x - scanLines[all_edgePairs[i].idx][all_edgePairs[i].data_0].pt3D.x, 2) +
|
||||
pow(scanLines[wheelTopPos.idx][wheelTopPos.data_0].pt3D.y - scanLines[all_edgePairs[i].idx][all_edgePairs[i].data_0].pt3D.y, 2));
|
||||
if (dist > computeLen)
|
||||
{
|
||||
endIdx = i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (endIdx < 0)
|
||||
endIdx = (int)all_edgePairs.size() - 1;
|
||||
|
||||
std::vector<SSG_intPair> edgePairs;
|
||||
for (int i = startIdx; i <= endIdx; i++)
|
||||
edgePairs.push_back(all_edgePairs[i]);
|
||||
|
||||
//提取轮毂特征：V型槽。连长6mm，角度最大的V型槽
|
||||
std::vector< SVzNL2DPoint> upWheelPos;
|
||||
std::vector< SVzNL2DPoint> downWheelPos;
|
||||
int startLine = arcFittingPos[0].x;
|
||||
int endLine = arcFittingPos.back().x;
|
||||
int startLine = edgePairs[0].idx;
|
||||
int endLine = edgePairs.back().idx;
|
||||
SSG_cornerParam wheelCornerPara;
|
||||
memset(&wheelCornerPara, 0, sizeof(SSG_cornerParam));
|
||||
wheelCornerPara.scale = 6.0;
|
||||
wheelCornerPara.scale = 5.0;
|
||||
wheelCornerPara.cornerTh = cornerPara.cornerTh;
|
||||
for (int line = startLine; line <= endLine; line++)
|
||||
{
|
||||
@ -471,26 +862,94 @@ WD_wheelArchInfo wd_wheelArchHeigthMeasure(
|
||||
1, //车轮的ID为1
|
||||
wheelCornerPara, //scale通常取bagH的1/4
|
||||
cornerFeatures);
|
||||
//取第一个为up
|
||||
if (cornerFeatures.size() >= 2)
|
||||
|
||||
int pairIdx = line - startLine;
|
||||
SSG_intPair& a_pair = edgePairs[pairIdx];
|
||||
int interval = (a_pair.data_1 - a_pair.data_0) / 3;
|
||||
if (cornerFeatures.size() >= 0)
|
||||
{
|
||||
upWheelPos.push_back(cornerFeatures[0].jumpPos2D);
|
||||
downWheelPos.push_back(cornerFeatures.back().jumpPos2D);
|
||||
for (int m = 0; m < (int)cornerFeatures.size(); m++)
|
||||
{
|
||||
|
||||
if ((cornerFeatures[m].jumpPos2D.y > (a_pair.data_0 + safeGuardLen)) &&
|
||||
(cornerFeatures[m].jumpPos2D.y < (a_pair.data_0 + interval)) )
|
||||
|
||||
{
|
||||
upWheelPos.push_back(cornerFeatures[m].jumpPos2D);
|
||||
break;
|
||||
}
|
||||
}
|
||||
for (int m = (int)cornerFeatures.size() - 1; m << (int)cornerFeatures.size() >= 0; m--)
|
||||
{
|
||||
if ((cornerFeatures[m].jumpPos2D.y < (a_pair.data_1 - safeGuardLen)) &&
|
||||
(cornerFeatures[m].jumpPos2D.y > (a_pair.data_1 - interval)))
|
||||
{
|
||||
downWheelPos.push_back(cornerFeatures[m].jumpPos2D);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
SVzNL2DPoint upPos;
|
||||
SVzNL3DPoint upWheelPt = _getWheelArcFittingPoint(scanLines, 4, upWheelPos, upPos);
|
||||
SVzNL2DPoint downPos;
|
||||
SVzNL3DPoint downWheelPt = _getWheelArcFittingPoint(scanLines, 5, downWheelPos, downPos);
|
||||
//计算直线
|
||||
double _a, _b, _c;
|
||||
compute2ptLine( downWheelPt, upWheelPt, &_a, &_b, &_c);
|
||||
double norm = sqrt(_a * _a + _b * _b);
|
||||
_a = _a / norm;
|
||||
_b = _b / norm;
|
||||
_c = _c / norm;
|
||||
|
||||
//寻找轮眉点
|
||||
//(1)快速寻找下端点（2）取中间区域，精确确定端点（3）抛物线拟合（4）计算轮眉点（最高点）
|
||||
std::vector<SVzNL2DPoint> arcEndings;
|
||||
_getArcEndings(scanLines, 2, arcEndings); //轮眉的ID是2
|
||||
if (arcEndings.size() == 0)
|
||||
{
|
||||
*errCode = SX_ERR_INVALID_ARC;
|
||||
return result;
|
||||
}
|
||||
//取与直线距离最小的点为轮眉点
|
||||
double minDist = -1;
|
||||
SVzNL2DPoint arcPos;
|
||||
for (int i = 0; i < (int)arcEndings.size(); i++)
|
||||
{
|
||||
SVzNL3DPoint a_pt = scanLines[arcEndings[i].x][arcEndings[i].y].pt3D;
|
||||
double dist = abs(a_pt.x * _a + a_pt.y * _b + _c);
|
||||
if (minDist < -1e-4)
|
||||
{
|
||||
minDist = dist;
|
||||
arcPos = arcEndings[i];
|
||||
}
|
||||
else if (minDist > dist)
|
||||
{
|
||||
minDist = dist;
|
||||
arcPos = arcEndings[i];
|
||||
}
|
||||
}
|
||||
if (minDist < -1e-4)
|
||||
{
|
||||
*errCode = SX_ERR_NO_WHEEL_ARC;
|
||||
return result;
|
||||
}
|
||||
//在XY平面生成拟合点
|
||||
double outLineLen = 200;
|
||||
SVzNL3DPoint arcPt = scanLines[arcPos.x][arcPos.y].pt3D;
|
||||
result.wheelArchPos = arcPt;
|
||||
result.arcLine[0] = { arcPt.x - outLineLen, arcPt.y, arcPt.z };
|
||||
result.arcLine[1] = { arcPt.x + outLineLen, arcPt.y, arcPt.z };
|
||||
result.wheelUpPos = upWheelPt;
|
||||
result.upLine[0] = { upWheelPt.x - outLineLen, upWheelPt.y, upWheelPt.z };
|
||||
result.upLine[1] = { upWheelPt.x + outLineLen, upWheelPt.y, upWheelPt.z };
|
||||
SVzNL2DPoint downPos;
|
||||
SVzNL3DPoint downWheelPt = _getWheelArcFittingPoint(scanLines, 5, downWheelPos, downPos);
|
||||
result.wheelDownPos = downWheelPt;
|
||||
result.downLine[0] = { downWheelPt.x - outLineLen, downWheelPt.y, downWheelPt.z };
|
||||
result.downLine[1] = { downWheelPt.x + outLineLen, downWheelPt.y, downWheelPt.z };
|
||||
double centerY = (upWheelPt.y + downWheelPt.y) / 2;
|
||||
int searchLine = (upPos.x + downPos.x) / 2;
|
||||
double minDist = DBL_MAX;
|
||||
minDist = DBL_MAX;
|
||||
int minPtIdx = 0;
|
||||
for (int i = 0; i < (int)scanLines[searchLine].size(); i++)
|
||||
{
|
||||
|
||||
@ -960,17 +960,21 @@ 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--)
|
||||
{
|
||||
if ((pntDirAngles_v[line][seedPos.y].type >= 0) && (abs(pntDirAngles_v[line][seedPos.y].corner) < pntCornenrMax))
|
||||
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;
|
||||
@ -980,7 +984,9 @@ bool _getZMaxPeakROI(SVzNL2DPoint seedPos,
|
||||
int right = -1;
|
||||
for (int line = seedPos.x; line < lineNum; line++)
|
||||
{
|
||||
if ((pntDirAngles_v[line][seedPos.y].type >= 0) && (abs(pntDirAngles_v[line][seedPos.y].corner) < pntCornenrMax))
|
||||
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;
|
||||
@ -990,7 +996,9 @@ bool _getZMaxPeakROI(SVzNL2DPoint seedPos,
|
||||
int top = -1;
|
||||
for (int ptIdx = seedPos.y; ptIdx >= 0; ptIdx--)
|
||||
{
|
||||
if ((pntDirAngles_h[ptIdx][seedPos.x].type >= 0) && (abs(pntDirAngles_h[ptIdx][seedPos.x].corner) < pntCornenrMax))
|
||||
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;
|
||||
@ -1000,7 +1008,9 @@ bool _getZMaxPeakROI(SVzNL2DPoint seedPos,
|
||||
int bottom = -1;
|
||||
for (int ptIdx = seedPos.y; ptIdx < ptNum; ptIdx++)
|
||||
{
|
||||
if ((pntDirAngles_h[ptIdx][seedPos.x].type >= 0) && (abs(pntDirAngles_h[ptIdx][seedPos.x].corner) < pntCornenrMax))
|
||||
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;
|
||||
@ -1069,13 +1079,12 @@ void wd_HolePositioning(
|
||||
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 == 152)
|
||||
if (line == 1062)
|
||||
int kkk = 1;
|
||||
|
||||
std::vector<SVzNL3DPosition>& lineData = scanLines[line];
|
||||
@ -1159,19 +1168,28 @@ void wd_HolePositioning(
|
||||
{
|
||||
//判断ROI
|
||||
SVzNLRect a_roi;
|
||||
bool validPk = _getZMaxPeakROI(a_lineZMax[j].jumpPos2D, pntDirAngles_v, pntDirAngles_h, flagCornerMax, 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))
|
||||
if ( (z_diff > minZPeakHeight) && (xLen < maxHoleDiameter) && (yLen < maxHoleDiameter) && (zMaxMin < planeInlierDistTh*2))
|
||||
{
|
||||
_zMaxInfo a_info;
|
||||
a_info.flag = 0;
|
||||
|
||||
@ -2637,9 +2637,9 @@ void _outputScanDataFile_removeZeros(char* fileName, SVzNL3DLaserLine * scanData
|
||||
|
||||
#define TEST_CONVERT_TO_GRID 0
|
||||
#define TEST_COMPUTE_WHEEL_ARCH 1
|
||||
#define TEST_COMPUTE_CALIB_PARA 1
|
||||
#define TEST_COMPUTE_CALIB_PARA 0
|
||||
|
||||
#define TEST_GROUP 1
|
||||
#define TEST_GROUP 2
|
||||
int main()
|
||||
{
|
||||
#if TEST_CONVERT_TO_GRID
|
||||
@ -2716,11 +2716,12 @@ int main()
|
||||
|
||||
#if TEST_COMPUTE_WHEEL_ARCH
|
||||
const char* dataPath[TEST_GROUP] = {
|
||||
"F:/ShangGu/项目/冠钦_轮眉高度测量/测试数据/现场数据/", //0
|
||||
"F:/ShangGu/项目/冠钦_轮眉高度测量/测试数据/现场数据/", //0
|
||||
"F:/ShangGu/项目/冠钦_轮眉高度测量/测试数据/部署现场数据/", //1
|
||||
};
|
||||
|
||||
SVzNLRange fileIdx[TEST_GROUP] = {
|
||||
{1,2},
|
||||
{1,2}, {1,4}
|
||||
};
|
||||
|
||||
SSG_planeCalibPara poseCalibPara;
|
||||
@ -2744,14 +2745,14 @@ int main()
|
||||
char _scan_file[256];
|
||||
|
||||
int endGroup = TEST_GROUP - 1;
|
||||
for (int grp = 0; grp <= endGroup; grp++)
|
||||
for (int grp = 1; grp <= endGroup; grp++)
|
||||
{
|
||||
char calibFile[250];
|
||||
sprintf_s(calibFile, "%sground_calib_para.txt", dataPath[grp]);
|
||||
poseCalibPara = _readCalibPara(calibFile);
|
||||
for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
|
||||
{
|
||||
//fidx = 193;
|
||||
//fidx = 4;
|
||||
sprintf_s(_scan_file, "%sLaserData_%d.txt", dataPath[grp], fidx);
|
||||
std::vector<std::vector< SVzNL3DPosition>> scanData;
|
||||
vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanData);
|
||||
@ -2783,17 +2784,17 @@ int main()
|
||||
|
||||
SSG_treeGrowParam growParam;
|
||||
growParam.maxLineSkipNum = 10;
|
||||
growParam.yDeviation_max = 5.0;
|
||||
growParam.maxSkipDistance = 5.0;
|
||||
growParam.zDeviation_max = 2.0;//
|
||||
growParam.yDeviation_max = 10.0;
|
||||
growParam.maxSkipDistance = 10.0;
|
||||
growParam.zDeviation_max = 10.0;//
|
||||
growParam.minLTypeTreeLen = 100; //mm
|
||||
growParam.minVTypeTreeLen = 100; //mm
|
||||
|
||||
WD_wheelArchInfo wheelArcHeight;
|
||||
memset(&wheelArcHeight, 0, sizeof(WD_wheelArchInfo));
|
||||
|
||||
bool wheePresense = wd_wheelPresenseDetection( scanData, wheelRoi3d);
|
||||
if (true == wheePresense)
|
||||
bool wheelPresense = wd_wheelPresenseDetection( scanData, wheelRoi3d);
|
||||
if (true == wheelPresense)
|
||||
{
|
||||
int lineNum = (int)scanData.size();
|
||||
for (int i = 0; i < lineNum; i++)
|
||||
@ -2802,7 +2803,7 @@ int main()
|
||||
int kkk = 1;
|
||||
//行处理
|
||||
//调平，去除地面
|
||||
wd_horizonCamera_lineDataR(scanData[i], poseCalibPara.planeCalib, poseCalibPara.planeHeight - 5);
|
||||
wd_horizonCamera_lineDataR(scanData[i], poseCalibPara.planeCalib, -1); // poseCalibPara.planeHeight - 5);
|
||||
}
|
||||
#if 0 //数据转存
|
||||
sprintf_s(_scan_file, "%sLaserLine%d_grid_RTadjust.txt", dataPath[grp], fidx);
|
||||
@ -2819,17 +2820,23 @@ int main()
|
||||
growParam,
|
||||
poseCalibPara,
|
||||
&errCode);
|
||||
}
|
||||
|
||||
long t2 = GetTickCount64();
|
||||
|
||||
|
||||
char _dbg_file[256];
|
||||
#if 1
|
||||
|
||||
sprintf_s(_dbg_file, "%sresult\\LaserLine%d_result.txt", dataPath[grp], fidx);
|
||||
_outputScanDataFile_RGBD_obj(_dbg_file, scanData, 0, 0, 0, wheelArcHeight);
|
||||
sprintf_s(_dbg_file, "%sresult\\LaserLine%d_result.txt", dataPath[grp], fidx);
|
||||
_outputScanDataFile_RGBD_obj(_dbg_file, scanData, 0, 0, 0, wheelArcHeight);
|
||||
#endif
|
||||
printf("%s: height=%f, arcToGrund=%f, time=%d(ms)!\n", _scan_file, wheelArcHeight.archToCenterHeigth, wheelArcHeight.archToGroundHeigth, (int)(t2 - t1));
|
||||
printf("%s: height=%f, arcToGrund=%f, time=%d(ms)!\n", _scan_file, wheelArcHeight.archToCenterHeigth, wheelArcHeight.archToGroundHeigth, (int)(t2 - t1));
|
||||
}
|
||||
else
|
||||
{
|
||||
printf("%s: no Wheel!", _scan_file);
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
@ -811,26 +811,29 @@ void TuoPuFa_holePosition_test(void)
|
||||
}
|
||||
|
||||
//华航孔定位
|
||||
#define HH_TEST_GROUP 4
|
||||
#define HH_TEST_GROUP 6
|
||||
void HuaHang_holePosition_test(void)
|
||||
{
|
||||
const char* dataPath[HH_TEST_GROUP] = {
|
||||
|
||||
"F:/ShangGu/项目/冠钦项目/华航孔定位/圆孔识别_数据1/", //0
|
||||
"F:/ShangGu/项目/冠钦项目/华航孔定位/圆孔识别_数据2/1/", //1
|
||||
"F:/ShangGu/项目/冠钦项目/华航孔定位/圆孔识别_数据2/2/", //1
|
||||
"F:/ShangGu/项目/冠钦项目/华航孔定位/圆孔识别_数据2/3/", //1
|
||||
"F:/ShangGu/项目/冠钦项目/华航孔定位/圆孔识别_数据2/2/", //2
|
||||
"F:/ShangGu/项目/冠钦项目/华航孔定位/圆孔识别_数据2/3/", //3
|
||||
"F:/ShangGu/项目/冠钦项目/华航孔定位/圆孔识别_数据4/好的/", //4
|
||||
"F:/ShangGu/项目/冠钦项目/华航孔定位/圆孔识别_数据4/坏的/", //5
|
||||
};
|
||||
|
||||
SVzNLRange fileIdx[HH_TEST_GROUP] = {
|
||||
{1,15}, {1, 21},{1,19}, {1,13}
|
||||
{1,15}, {1, 21},{1,19}, {1,13},
|
||||
{1, 4}, {4,13},
|
||||
};
|
||||
|
||||
const char* ver = wd_workpieceHolePositioningVersion();
|
||||
printf("ver:%s\n", ver);
|
||||
|
||||
#if TEST_COMPUTE_HOLE
|
||||
for (int grp = 0; grp <= 3; grp++)
|
||||
for (int grp = 5; grp <= 5; grp++)
|
||||
{
|
||||
for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
|
||||
{
|
||||
@ -851,7 +854,7 @@ void HuaHang_holePosition_test(void)
|
||||
|
||||
SSG_cornerParam cornerParam;
|
||||
cornerParam.cornerTh = 60; //45度角
|
||||
cornerParam.scale = 4; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8;
|
||||
cornerParam.scale = 10; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8;
|
||||
cornerParam.minEndingGap = 10; // algoParam.bagParam.bagW / 4;
|
||||
cornerParam.minEndingGap_z = 5.0;
|
||||
cornerParam.jumpCornerTh_1 = 15; //水平角度，小于此角度视为水平
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user