987 lines
30 KiB
C++
987 lines
30 KiB
C++
#include <vector>
|
||
#include "SG_baseDataType.h"
|
||
#include "SG_baseAlgo_Export.h"
|
||
#include "wheelArchHeigthMeasure_Export.h"
|
||
#include <opencv2/opencv.hpp>
|
||
#include <limits>
|
||
|
||
//version 1.0.0 : base version release to customer
|
||
//version 1.1.0 : 添加了轮眉到地面高度输出
|
||
//version 1.2.0 : 修正了地面调平，防止法向量旋转180度
|
||
//version 1.3.0 : 修正了寻找上下点使用固定门限的问题
|
||
//version 1.3.1 : 添加了一个检测轮毂是否存在的函数
|
||
//version 1.3.2 : 针对不理想点云改进了算法，增强鲁棒性
|
||
//version 1.3.3 : 轮眉点的提取进行了改进，修正了可能的取点错误
|
||
//version 1.3.4 : 轮眉到轮心高度计算方法进行了修正，由Y高度差修改为两点距离
|
||
std::string m_strVersion = "1.3.4";
|
||
const char* wd_wheelArchHeigthMeasureVersion(void)
|
||
{
|
||
return m_strVersion.c_str();
|
||
}
|
||
|
||
//相机水平安装计算地面调平参数。
|
||
//相机Z轴基本平行地面时，需要以地面为参照，将相机调水平
|
||
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
|
||
SSG_planeCalibPara wd_horizonCamera_getGroundCalibPara(
|
||
std::vector< std::vector<SVzNL3DPosition>>& scanLines)
|
||
{
|
||
return sg_HCameraVScan_getGroundCalibPara(scanLines);
|
||
}
|
||
|
||
//相机水平时姿态调平，并去除地面
|
||
void wd_horizonCamera_lineDataR(
|
||
std::vector< SVzNL3DPosition>& a_line,
|
||
const double* camPoseR,
|
||
double groundH)
|
||
{
|
||
HCamera_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;
|
||
}
|
||
|
||
bool compareByPtSize(const SWD_clustersInfo& a, const SWD_clustersInfo& b) {
|
||
return a.ptSize > b.ptSize;
|
||
}
|
||
|
||
//提取轮眉区域的下端点
|
||
void _getArcEndings(std::vector< std::vector<SVzNL3DPosition>>& scanLines, const int cluster_arc_id, std::vector<SVzNL2DPoint>& contourPts, double maxZ)
|
||
{
|
||
int lineNum = (int)scanLines.size();
|
||
|
||
for (int i = 0; i < lineNum; i++)
|
||
{
|
||
int ptNum = (int)scanLines[i].size();
|
||
int lastIdx = -1;
|
||
for (int j = 0; j < ptNum; j++)
|
||
{
|
||
if ((i == 380) && (j > 288))
|
||
int kkk = 1;
|
||
if ( (scanLines[i][j].nPointIdx == cluster_arc_id) && (scanLines[i][j].pt3D.z >1e-4) && (scanLines[i][j].pt3D.z < maxZ))
|
||
lastIdx = j;
|
||
}
|
||
if (lastIdx >= 0)
|
||
{
|
||
SVzNL2DPoint a_pt = { i, lastIdx};
|
||
contourPts.push_back(a_pt);
|
||
}
|
||
}
|
||
}
|
||
|
||
//提取轮眉种子点，定义为y最小的端点
|
||
int _getArcEndingsCenterPos(std::vector< std::vector<SVzNL3DPosition>>& scanLines, std::vector<SVzNL2DPoint>& contourPts)
|
||
{
|
||
double minY = DBL_MAX;
|
||
int seedPosIdx = 0;
|
||
for (int i = 0, i_max = (int)contourPts.size(); i < i_max; i++)
|
||
{
|
||
SVzNL2DPoint& a_pos = contourPts[i];
|
||
double y = scanLines[a_pos.x][a_pos.y].pt3D.y;
|
||
if ((minY > y) && (scanLines[a_pos.x][a_pos.y].pt3D.z > 1e-4))
|
||
{
|
||
seedPosIdx = i;
|
||
minY = y;
|
||
}
|
||
}
|
||
return seedPosIdx;
|
||
}
|
||
|
||
//使用端点直线，检查点到直线的距离，大于门限的分割
|
||
int computeMaxDistPos(
|
||
int ptStartIdx, int ptEndIdx,
|
||
std::vector< SVzNL3DPosition>& lineData)
|
||
{
|
||
SVzNL3DPoint pt1 = lineData[ptStartIdx].pt3D;
|
||
SVzNL3DPoint pt2 = lineData[ptEndIdx].pt3D;
|
||
if ((pt1.z < 1e-4) || (pt2.z < 1e-4))
|
||
return -1;
|
||
double _a, _b, _c;
|
||
compute2ptLine_2(
|
||
pt1.y, pt1.z,
|
||
pt2.y, pt2.z,
|
||
&_a, &_b, &_c);
|
||
//compute2ptLine(pt1, pt2, &_a, &_b, &_c);
|
||
double denominator = sqrt(_a * _a + _b * _b);
|
||
//归一化
|
||
_a = _a / denominator;
|
||
_b = _b / denominator;
|
||
_c = _c / denominator;
|
||
|
||
double maxDist = 0;
|
||
int maxPos = 0;
|
||
for (int i = ptStartIdx; i <= ptEndIdx; i++)
|
||
{
|
||
SVzNL3DPoint a_pt = lineData[i].pt3D;
|
||
if (a_pt.z > 1e-4)
|
||
{
|
||
double dist = abs(a_pt.y * _a + a_pt.z * _b + _c);
|
||
if (maxDist < dist)
|
||
{
|
||
maxDist = dist;
|
||
maxPos = i;
|
||
}
|
||
}
|
||
}
|
||
return maxPos;
|
||
}
|
||
|
||
void _extractArcFittingPts(
|
||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||
std::vector<SVzNL2DPoint>& contourPts,
|
||
double chkWin,
|
||
int seedPosIdx,
|
||
std::vector< SVzNL2DPoint>& arcFittingPos)
|
||
{
|
||
int size = (int)contourPts.size();
|
||
arcFittingPos.push_back(contourPts[seedPosIdx]);
|
||
SVzNL3DPoint seedPt = scanLines[contourPts[seedPosIdx].x][contourPts[seedPosIdx].y].pt3D;
|
||
for (int i = seedPosIdx - 1; i >= 0; i--)
|
||
{
|
||
SVzNL2DPoint chkPos = contourPts[i];
|
||
SVzNL3DPoint chkPt = scanLines[chkPos.x][chkPos.y].pt3D;
|
||
double len = sqrt(pow(chkPt.x - seedPt.x, 2) + pow(chkPt.y - seedPt.y, 2));
|
||
if (len > chkWin)
|
||
break;
|
||
arcFittingPos.insert(arcFittingPos.begin(), chkPos);
|
||
}
|
||
for (int i = seedPosIdx + 1; i < size; i++)
|
||
{
|
||
SVzNL2DPoint chkPos = contourPts[i];
|
||
SVzNL3DPoint chkPt = scanLines[chkPos.x][chkPos.y].pt3D;
|
||
double len = sqrt(pow(chkPt.x - seedPt.x, 2) + pow(chkPt.y - seedPt.y, 2));
|
||
if (len > chkWin)
|
||
break;
|
||
arcFittingPos.push_back(chkPos);
|
||
}
|
||
|
||
//检查正确的端点，检查20mm
|
||
double chkLen = 20;
|
||
for (int i = 0, i_max = (int)arcFittingPos.size(); i < i_max; i++)
|
||
{
|
||
SVzNL2DPoint chkPos = arcFittingPos[i];
|
||
SVzNL3DPoint chkPt = scanLines[chkPos.x][chkPos.y].pt3D;
|
||
int endIdx = chkPos.y;
|
||
int startIdx = endIdx;
|
||
for (int m = endIdx - 1; m >= 0; m--)
|
||
{
|
||
SVzNL3DPoint a_pt = scanLines[chkPos.x][m].pt3D;
|
||
if (a_pt.z > 1e-4)
|
||
{
|
||
double len = sqrt(pow(a_pt.y - chkPt.y, 2) + pow(a_pt.z - chkPt.z, 2));
|
||
if (len > chkLen)
|
||
break;
|
||
startIdx = m;
|
||
}
|
||
}
|
||
if (startIdx != endIdx)
|
||
{
|
||
int maxPos = computeMaxDistPos(startIdx, endIdx, scanLines[chkPos.x]);
|
||
SVzNL3DPoint max_pt = scanLines[chkPos.x][maxPos].pt3D;
|
||
double len1 = sqrt(pow(max_pt.y - chkPt.y, 2) + pow(max_pt.z - chkPt.z, 2));
|
||
//构建等腰三角形，重新寻找点到底边的距离最高点
|
||
for (int m = maxPos - 1; m >= 0; m--)
|
||
{
|
||
SVzNL3DPoint a_pt = scanLines[chkPos.x][m].pt3D;
|
||
if (a_pt.z > 1e-4)
|
||
{
|
||
double len = sqrt(pow(a_pt.y - max_pt.y, 2) + pow(a_pt.z - max_pt.z, 2));
|
||
if (len > len1)
|
||
break;
|
||
startIdx = m;
|
||
}
|
||
}
|
||
maxPos = computeMaxDistPos(startIdx, endIdx, scanLines[chkPos.x]);
|
||
arcFittingPos[i].y = maxPos;
|
||
}
|
||
}
|
||
|
||
}
|
||
|
||
SVzNL3DPoint _getWheelArcFittingPoint(
|
||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||
const int maskID,
|
||
std::vector< SVzNL2DPoint>& fittingPos,
|
||
SVzNL2DPoint& fittingPosition)
|
||
{
|
||
//upWheel, 在XY平面生成拟合点
|
||
std::vector<cv::Point2d> fittingPoints;
|
||
for (int i = 0, i_max = (int)fittingPos.size(); i < i_max; i++)
|
||
{
|
||
int lineIdx = fittingPos[i].x;
|
||
int ptIdx = fittingPos[i].y;
|
||
cv::Point2d a_pt = { scanLines[lineIdx][ptIdx].pt3D.x,scanLines[lineIdx][ptIdx].pt3D.y };
|
||
scanLines[lineIdx][ptIdx].nPointIdx = maskID;
|
||
fittingPoints.push_back(a_pt);
|
||
}
|
||
|
||
double a, b, c, mse, max_err;
|
||
bool fitResult = leastSquareParabolaFitEigen(
|
||
fittingPoints, a, b, c, mse, max_err);
|
||
//计算轮毂上顶点
|
||
SVzNL3DPoint fitPt;
|
||
fitPt.x = -b / (2 * a);
|
||
fitPt.y = (4 * a * c - b * b) / (4 * a);
|
||
//在轮廓点中寻找最近的点
|
||
SVzNL2DPoint subOptiPtPos = { 0,0 };
|
||
double minLen = DBL_MAX;
|
||
for (int i = 0, i_max = (int)fittingPos.size(); i < i_max; i++)
|
||
{
|
||
int lineIdx = fittingPos[i].x;
|
||
int ptIdx = fittingPos[i].y;
|
||
cv::Point2d a_pt = { scanLines[lineIdx][ptIdx].pt3D.x,scanLines[lineIdx][ptIdx].pt3D.y };
|
||
double len = sqrt(pow(fitPt.x - a_pt.x, 2) + pow(fitPt.y - a_pt.y, 2));
|
||
if (minLen > len)
|
||
{
|
||
subOptiPtPos = fittingPos[i];
|
||
minLen = len;
|
||
}
|
||
}
|
||
fittingPosition = subOptiPtPos;
|
||
fitPt.z = scanLines[subOptiPtPos.x][subOptiPtPos.y].pt3D.z;
|
||
return fitPt;
|
||
}
|
||
|
||
|
||
bool wd_wheelPresenseDetection(
|
||
std::vector<std::vector< SVzNL3DPosition>>& scanLines,
|
||
const SVzNL3DRangeD wheelRoi3d)
|
||
{
|
||
int lines = (int)scanLines.size();
|
||
int validPtNum = 0;
|
||
for (int line = 0; line < lines; line++)
|
||
{
|
||
std::vector< SVzNL3DPosition>& a_line = scanLines[line];
|
||
int ptNum = (int)a_line.size();
|
||
for (int i = 0; i < ptNum; i++)
|
||
{
|
||
SVzNL3DPoint& a_pt3D = a_line[i].pt3D;
|
||
if ( (a_pt3D.x > wheelRoi3d.xRange.min) && (a_pt3D.x < wheelRoi3d.xRange.max)&&
|
||
(a_pt3D.y > wheelRoi3d.yRange.min) && (a_pt3D.y < wheelRoi3d.yRange.max)&&
|
||
(a_pt3D.z > wheelRoi3d.zRange.min) && (a_pt3D.z < wheelRoi3d.zRange.max) && (a_pt3D.z> 1e-4))
|
||
{
|
||
validPtNum++;
|
||
}
|
||
}
|
||
}
|
||
if (validPtNum > 1000)
|
||
return true;
|
||
else
|
||
return false;
|
||
|
||
}
|
||
|
||
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,
|
||
const SSG_cornerParam cornerPara,
|
||
const SSG_lineSegParam lineSegPara,
|
||
const SSG_outlierFilterParam filterParam,
|
||
const SSG_treeGrowParam growParam,
|
||
const SSG_planeCalibPara groundCalibPara,
|
||
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));
|
||
|
||
int lineNum = (int)scanLines.size();
|
||
int linePtNum = (int)scanLines[0].size();
|
||
bool isGridData = true;
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
std::vector<SVzNL3DPosition>& lineData = scanLines[line];
|
||
if (linePtNum != (int)lineData.size())
|
||
isGridData = false;
|
||
//滤波，滤除异常点
|
||
sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam);
|
||
}
|
||
|
||
//生成水平扫描
|
||
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特征提取
|
||
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<std::vector<SSG_featureClusteringInfo>> featureInfoMask;
|
||
std::vector<std::vector<SVzNL3DPoint>> feature3DInfo;
|
||
featureInfoMask.resize(lineNum);
|
||
feature3DInfo.resize(lineNum);
|
||
for (int i = 0; i < lineNum; i++)
|
||
{
|
||
featureInfoMask[i].resize(linePtNum);
|
||
feature3DInfo[i].resize(linePtNum);
|
||
}
|
||
//生成Mask
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
std::vector<SVzNL3DPosition>& lineData = scanLines[line];
|
||
for (int ptIdx = 0; ptIdx < linePtNum; ptIdx++)
|
||
{
|
||
if (scanLines[line][ptIdx].pt3D.z > 1e-4)
|
||
{
|
||
SSG_featureClusteringInfo a_mask;
|
||
memset(&a_mask, 0, sizeof(SSG_featureClusteringInfo));
|
||
a_mask.featurType = 1;
|
||
a_mask.lineIdx = line;
|
||
a_mask.ptIdx = ptIdx;
|
||
featureInfoMask[line][ptIdx] = a_mask;
|
||
feature3DInfo[line][ptIdx] = scanLines[line][ptIdx].pt3D;
|
||
}
|
||
}
|
||
}
|
||
//聚类
|
||
//采用迭代思想，回归思路进行高效聚类
|
||
std::vector<std::vector< SVzNL2DPoint>> clusters; //只记录位置
|
||
std::vector<SWD_clustersInfo> clustersInfo;
|
||
int clusterID = 1;
|
||
int clusterCheckWin = 5;
|
||
for (int y = 0; y < linePtNum; y++)
|
||
{
|
||
for (int x = 0; x < lineNum; x++)
|
||
{
|
||
SSG_featureClusteringInfo& a_featureInfo = featureInfoMask[x][y];
|
||
if ((0 == a_featureInfo.featurType) || (a_featureInfo.clusterID > 0)) //非特征或已经处理
|
||
continue;
|
||
|
||
SVzNL3DPoint& a_feature3DValue = feature3DInfo[x][y];
|
||
SVzNL3DRangeD a_clusterRoi;
|
||
a_clusterRoi.xRange.min = a_feature3DValue.x;
|
||
a_clusterRoi.xRange.max = a_feature3DValue.x;
|
||
a_clusterRoi.yRange.min = a_feature3DValue.y;
|
||
a_clusterRoi.yRange.max = a_feature3DValue.y;
|
||
a_clusterRoi.zRange.min = a_feature3DValue.z;
|
||
a_clusterRoi.zRange.max = a_feature3DValue.z;
|
||
|
||
SVzNL2DPoint a_seedPos = { x, y };
|
||
std::vector< SVzNL2DPoint> a_cluster;
|
||
a_cluster.push_back(a_seedPos);
|
||
wd_gridPointClustering(
|
||
featureInfoMask,//int，记录特征标记和clusterID，附加一个flag
|
||
feature3DInfo,//double,记录坐标信息
|
||
clusterCheckWin, //搜索窗口
|
||
growParam,//聚类条件
|
||
clusterID, //当前Cluster的ID
|
||
a_cluster, //result
|
||
a_clusterRoi
|
||
);
|
||
clusters.push_back(a_cluster);
|
||
SWD_clustersInfo a_info;
|
||
a_info.clusterIdx = clusterID;
|
||
a_info.ptSize = (int)a_cluster.size();
|
||
a_info.roi3D = a_clusterRoi;
|
||
clustersInfo.push_back(a_info);
|
||
clusterID++;
|
||
}
|
||
}
|
||
//聚类结果分析
|
||
//取最大的两个聚类。上面的聚类是前盖板，下面的是车轮
|
||
std::sort(clustersInfo.begin(), clustersInfo.end(), compareByPtSize);
|
||
int cluseter_wheel_id, cluster_arc_id;
|
||
if (clustersInfo[0].roi3D.yRange.max > clustersInfo[1].roi3D.yRange.max)
|
||
{
|
||
cluseter_wheel_id = clustersInfo[0].clusterIdx;
|
||
cluster_arc_id = clustersInfo[1].clusterIdx;
|
||
}
|
||
else
|
||
{
|
||
cluseter_wheel_id = clustersInfo[1].clusterIdx;
|
||
cluster_arc_id = clustersInfo[0].clusterIdx;
|
||
}
|
||
std::vector< SVzNL2DPoint>& cluster_wheel = clusters[cluseter_wheel_id - 1];
|
||
std::vector< SVzNL2DPoint>& cluster_arc = clusters[cluster_arc_id - 1];
|
||
for (int i = 0, i_max = (int)cluster_wheel.size(); i < i_max; i++)
|
||
{
|
||
int lineIdx = cluster_wheel[i].x;
|
||
int ptIdx = cluster_wheel[i].y;
|
||
scanLines[lineIdx][ptIdx].nPointIdx = 1; //wheel
|
||
}
|
||
for (int i = 0, i_max = (int)cluster_arc.size(); i < i_max; i++)
|
||
{
|
||
int lineIdx = cluster_arc[i].x;
|
||
int ptIdx = cluster_arc[i].y;
|
||
scanLines[lineIdx][ptIdx].nPointIdx = 2;
|
||
}
|
||
|
||
std::vector<std::vector<SWDIndexingVzPoint>> upEndings;
|
||
std::vector<std::vector<SWDIndexingVzPoint>> downEndings;
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
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;
|
||
}
|
||
//以最上面的为轮子边沿
|
||
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 = edgePairs[0].idx;
|
||
int endLine = edgePairs.back().idx;
|
||
SSG_cornerParam wheelCornerPara;
|
||
memset(&wheelCornerPara, 0, sizeof(SSG_cornerParam));
|
||
wheelCornerPara.scale = 5.0;
|
||
wheelCornerPara.cornerTh = cornerPara.cornerTh;
|
||
for (int line = startLine; line <= endLine; line++)
|
||
{
|
||
std::vector<SSG_basicFeature1D> cornerFeatures;
|
||
sg_maskData_getLineCornerFeature(
|
||
scanLines[line],
|
||
line,
|
||
1, //车轮的ID为1
|
||
wheelCornerPara, //scale通常取bagH的1/4
|
||
cornerFeatures);
|
||
|
||
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)
|
||
{
|
||
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, upWheelPt.z); //轮眉的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 };
|
||
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;
|
||
minDist = DBL_MAX;
|
||
int minPtIdx = 0;
|
||
for (int i = 0; i < (int)scanLines[searchLine].size(); i++)
|
||
{
|
||
if (scanLines[searchLine][i].pt3D.z > 1e-4)
|
||
{
|
||
double dist = abs(scanLines[searchLine][i].pt3D.y - centerY);
|
||
if (minDist > dist)
|
||
{
|
||
minDist = dist;
|
||
minPtIdx = i;
|
||
}
|
||
}
|
||
}
|
||
result.centerLine[0] = { downWheelPt.x - outLineLen, centerY, scanLines[searchLine][minPtIdx].pt3D.z };
|
||
result.centerLine[1] = { downWheelPt.x + outLineLen, centerY, scanLines[searchLine][minPtIdx].pt3D.z };
|
||
result.archToCenterHeigth = sqrt(pow(centerY - arcPt.y, 2) + pow(scanLines[searchLine][minPtIdx].pt3D.z - arcPt.z, 2)); // centerY - arcPt.y;
|
||
result.archToGroundHeigth = groundCalibPara.planeHeight - arcPt.y;
|
||
//将数据重新投射回原来的坐标系，以保持手眼标定结果正确
|
||
for (int i = 0; i < lineNum; i++)
|
||
lineDataRT_vector(scanLines[i], groundCalibPara.invRMatrix, -1);
|
||
//将检测结果重新投射回原来的坐标系
|
||
result.wheelArchPos = _ptRotate(result.wheelArchPos, groundCalibPara.invRMatrix);
|
||
result.wheelUpPos = _ptRotate(result.wheelUpPos, groundCalibPara.invRMatrix);
|
||
result.wheelDownPos = _ptRotate(result.wheelDownPos, groundCalibPara.invRMatrix);
|
||
for (int i = 0; i < 2; i++)
|
||
{
|
||
result.arcLine[i] = _ptRotate(result.arcLine[i], groundCalibPara.invRMatrix);
|
||
result.centerLine[i] = _ptRotate(result.centerLine[i], groundCalibPara.invRMatrix);
|
||
result.downLine[i] = _ptRotate(result.downLine[i], groundCalibPara.invRMatrix);
|
||
result.upLine[i] = _ptRotate(result.upLine[i], groundCalibPara.invRMatrix);
|
||
}
|
||
return result;
|
||
} |