811 lines
22 KiB
C++
811 lines
22 KiB
C++
#include <vector>
|
||
#include "SG_baseDataType.h"
|
||
#include "SG_baseAlgo_Export.h"
|
||
#include "WD_QRcode3Ddetection_Export.h"
|
||
#include <opencv2/opencv.hpp>
|
||
#include <limits>
|
||
|
||
typedef struct
|
||
{
|
||
int id;
|
||
double dist;
|
||
double angle;
|
||
}_linkInfo;
|
||
typedef struct
|
||
{
|
||
int objId;
|
||
SVzNL3DPoint ptPos;
|
||
_linkInfo link[8]; //R, RT, T, LT, L, LB, B, RB
|
||
}NodeOctLink;
|
||
|
||
typedef struct
|
||
{
|
||
SSG_ROIRectD roi;
|
||
int* qrCode;
|
||
}DMCodeInfo;
|
||
|
||
//将ply格式的数据恢复成扫描行的数据形式，从而方面按行进行处理
|
||
void wd_getScanLines(
|
||
std::vector<SVzNL3DPoint>& scanData,
|
||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||
int scan_rows)
|
||
{
|
||
std::vector<SVzNL3DPosition> a_line;
|
||
for (int i = 0, i_max = scanData.size(); i < i_max; i++)
|
||
{
|
||
int idx = i % scan_rows;
|
||
if (0 == idx)
|
||
{
|
||
//新的一行
|
||
if (a_line.size() > 0)
|
||
{
|
||
scanLines.push_back(a_line);
|
||
a_line.clear();
|
||
}
|
||
}
|
||
SVzNL3DPoint a_pt = scanData[i];
|
||
SVzNL3DPosition a_idxPt;
|
||
a_idxPt.nPointIdx = 0;
|
||
a_idxPt.pt3D = a_pt;
|
||
a_line.push_back(a_idxPt);
|
||
}
|
||
if(a_line.size() > 0)
|
||
scanLines.push_back(a_line);
|
||
return;
|
||
}
|
||
|
||
SSG_planeCalibPara wd_getBaseCalibPara(
|
||
SVzNL3DLaserLine* laser3DPoints,
|
||
int lineNum)
|
||
{
|
||
return sg_getPlaneCalibPara(laser3DPoints, lineNum);
|
||
}
|
||
|
||
void wd_lineDataR(std::vector< SVzNL3DPosition>& a_line,
|
||
const double* camPoseR,
|
||
double groundH)
|
||
{
|
||
lineDataRT_vector(a_line, camPoseR, groundH);
|
||
}
|
||
|
||
SVzNL3DPoint _getObjCenter(SSG_featureTree& a_tree)
|
||
{
|
||
SVzNL3DPoint centerPt = { 0, 0, 0 };
|
||
int nodeNum = 0;
|
||
for (int i = 0; i < a_tree.treeNodes.size(); i++)
|
||
{
|
||
centerPt.x += a_tree.treeNodes[i].jumpPos.x;
|
||
centerPt.y += a_tree.treeNodes[i].jumpPos.y;
|
||
centerPt.z += a_tree.treeNodes[i].jumpPos.z;
|
||
nodeNum++;
|
||
}
|
||
if (nodeNum > 0)
|
||
{
|
||
centerPt.x = centerPt.x / nodeNum;
|
||
centerPt.y = centerPt.y / nodeNum;
|
||
centerPt.z = centerPt.z / nodeNum;
|
||
}
|
||
return centerPt;
|
||
}
|
||
|
||
_linkInfo compareBestLinl(_linkInfo link_1, _linkInfo link_2, double bestDist)
|
||
{
|
||
double dist_diff1 = abs(link_1.dist - bestDist);
|
||
double dist_diff2 = abs(link_2.dist - bestDist);
|
||
if (dist_diff1 < dist_diff2)
|
||
return link_1;
|
||
else
|
||
return link_2;
|
||
}
|
||
|
||
//建立Node间的上下左右的Link关系
|
||
void _createNodeLinks(
|
||
std::vector< SVzNL3DPosition>& a_cluster,
|
||
std::vector< NodeOctLink>& clusterNodeLinks,
|
||
double row_space,
|
||
double col_space
|
||
)
|
||
{
|
||
int dirInvTbl[8] = { 4, 5, 6, 7, 0, 1, 2, 3 };
|
||
double diaDist = sqrt(pow(row_space, 2) + pow(col_space, 2));
|
||
int nodeSize = a_cluster.size();
|
||
for (int i = 0; i < nodeSize; i++)
|
||
{
|
||
SVzNL3DPosition& a_node = a_cluster[i];
|
||
NodeOctLink& a_nodeLink = clusterNodeLinks[i];
|
||
_linkInfo link[8];
|
||
for (int j = 0; j < 8; j++)
|
||
{
|
||
link[j].id = -1;
|
||
link[j].dist = 0.0;
|
||
link[j].angle = 0.0;
|
||
}
|
||
for (int j = i+1; j < nodeSize; j++)
|
||
{
|
||
SVzNL3DPosition& chk_node = a_cluster[j];
|
||
NodeOctLink& chk_nodeLink = clusterNodeLinks[j];
|
||
if (chk_node.nPointIdx < 0)
|
||
continue;
|
||
|
||
double dist = sqrt(pow(a_node.pt3D.x - chk_node.pt3D.x, 2) + pow(a_node.pt3D.y - chk_node.pt3D.y, 2));
|
||
if (dist < diaDist * 2) //粗过滤
|
||
{
|
||
double angle = atan2(a_node.pt3D.y - chk_node.pt3D.y, chk_node.pt3D.x - a_node.pt3D.x); //图像坐标系y方向与欧氏坐标系相么
|
||
angle = angle * 180.0 / PI; //转为角度
|
||
if (angle < 0)
|
||
angle += 360; //转成0-360度
|
||
|
||
_linkInfo a_link = { j, dist, angle };
|
||
if ((angle > 345) || (angle < 15)) //R:0
|
||
{
|
||
if (dist < col_space * 1.5)
|
||
{
|
||
if (link[0].id < 0)
|
||
link[0] = a_link;
|
||
else
|
||
link[0] = compareBestLinl(link[0], a_link, col_space);
|
||
}
|
||
}
|
||
else if ((angle > 30) && (angle < 60)) //RT:1
|
||
{
|
||
if (dist < diaDist * 1.5)
|
||
{
|
||
if (link[1].id < 0)
|
||
link[1] = a_link;
|
||
else
|
||
link[1] = compareBestLinl(link[1], a_link, diaDist);
|
||
}
|
||
}
|
||
else if ((angle > 75) && (angle < 105)) //T:2
|
||
{
|
||
if (dist < row_space * 1.5)
|
||
{
|
||
if (link[2].id < 0)
|
||
link[2] = a_link;
|
||
else
|
||
link[2] = compareBestLinl(link[2], a_link, row_space);
|
||
}
|
||
}
|
||
else if ((angle > 120) && (angle < 150)) //LT:3
|
||
{
|
||
if (dist < diaDist * 1.5)
|
||
{
|
||
if (link[3].id < 0)
|
||
link[3] = a_link;
|
||
else
|
||
link[3] = compareBestLinl(link[3], a_link, diaDist);
|
||
}
|
||
}
|
||
else if ((angle > 165) && (angle < 195)) //L:4
|
||
{
|
||
if (dist < col_space * 1.5)
|
||
{
|
||
if (link[4].id < 0)
|
||
link[4] = a_link;
|
||
else
|
||
link[4] = compareBestLinl(link[4], a_link, col_space);
|
||
}
|
||
}
|
||
else if ((angle > 210) && (angle < 240)) //LB:5
|
||
{
|
||
if (dist < diaDist * 1.5)
|
||
{
|
||
if (link[5].id < 0)
|
||
link[5] = a_link;
|
||
else
|
||
link[5] = compareBestLinl(link[5], a_link, diaDist);
|
||
}
|
||
}
|
||
else if ((angle > 255) && (angle < 285)) //B:6
|
||
{
|
||
if (dist < row_space * 1.5)
|
||
{
|
||
if (link[6].id < 0)
|
||
link[6] = a_link;
|
||
else
|
||
link[6] = compareBestLinl(link[6], a_link, row_space);
|
||
}
|
||
}
|
||
else if ((angle > 300) && (angle < 330)) //RB:7
|
||
{
|
||
if (dist < diaDist * 1.5)
|
||
{
|
||
if (link[7].id < 0)
|
||
link[7] = a_link;
|
||
else
|
||
link[7] = compareBestLinl(link[7], a_link, diaDist);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
for (int j = 0; j < 8; j++)
|
||
{
|
||
if (link[j].id >= 0)
|
||
{
|
||
a_nodeLink.link[j] = link[j];
|
||
int linkId = link[j].id;
|
||
int dir_inv = dirInvTbl[j];
|
||
if (clusterNodeLinks[linkId].link[dir_inv].id < 0)
|
||
{
|
||
clusterNodeLinks[linkId].link[dir_inv].id = a_nodeLink.objId;
|
||
clusterNodeLinks[linkId].link[dir_inv].dist = link[j].dist;
|
||
clusterNodeLinks[linkId].link[dir_inv].angle = link[j].angle + 180;
|
||
if (clusterNodeLinks[linkId].link[dir_inv].angle >= 360)
|
||
clusterNodeLinks[linkId].link[dir_inv].angle = clusterNodeLinks[linkId].link[dir_inv].angle - 360;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
//
|
||
std::vector<int> _LTseedSearchRight(NodeOctLink& startNode, std::vector< NodeOctLink>& clusterNodeLinks)
|
||
{
|
||
NodeOctLink a_seed = startNode;
|
||
std::vector<int> result;
|
||
result.push_back(a_seed.objId);
|
||
while (1)
|
||
{
|
||
if (a_seed.link[0].id < 0)
|
||
break;
|
||
a_seed = clusterNodeLinks[a_seed.link[0].id];
|
||
|
||
if ((a_seed.link[1].id < 0) && (a_seed.link[2].id < 0) && (a_seed.link[3].id < 0))
|
||
result.push_back(a_seed.objId);
|
||
else
|
||
break;
|
||
}
|
||
return result;
|
||
}
|
||
std::vector<int> _LTseedSearchBottom(NodeOctLink& startNode, std::vector< NodeOctLink>& clusterNodeLinks)
|
||
{
|
||
NodeOctLink a_seed = startNode;
|
||
std::vector<int> result;
|
||
result.push_back(a_seed.objId);
|
||
while (1)
|
||
{
|
||
if (a_seed.link[6].id < 0)
|
||
break;
|
||
a_seed = clusterNodeLinks[a_seed.link[6].id];
|
||
|
||
if ((a_seed.link[3].id < 0) && (a_seed.link[4].id < 0) && (a_seed.link[5].id < 0))
|
||
result.push_back(a_seed.objId);
|
||
else
|
||
break;
|
||
}
|
||
return result;
|
||
}
|
||
int _foundDM_LTcorner(std::vector< NodeOctLink>& clusterNodeLinks, int rows, int cols)
|
||
{
|
||
for (int i = 0, i_max = clusterNodeLinks.size(); i < i_max; i++)
|
||
{
|
||
//寻找种子向右向下
|
||
NodeOctLink a_seed = clusterNodeLinks[i];
|
||
if ((a_seed.link[0].id >= 0) && (a_seed.link[6].id >= 0) &&
|
||
(a_seed.link[1].id < 0) && (a_seed.link[2].id < 0) &&
|
||
(a_seed.link[3].id < 0) && (a_seed.link[4].id < 0) &&
|
||
(a_seed.link[5].id < 0)) //合格的种子
|
||
{
|
||
std::vector<int> R_result = _LTseedSearchRight(a_seed, clusterNodeLinks);
|
||
std::vector<int> B_result = _LTseedSearchBottom(a_seed, clusterNodeLinks);
|
||
if (((int)R_result.size() == cols) && ((int)B_result.size() == rows)) //找到LT角点
|
||
return i;
|
||
}
|
||
}
|
||
return -1;
|
||
}
|
||
|
||
std::vector<int> _RTseedSearchLeft(NodeOctLink& startNode, std::vector< NodeOctLink>& clusterNodeLinks)
|
||
{
|
||
NodeOctLink a_seed = startNode;
|
||
std::vector<int> result;
|
||
result.push_back(a_seed.objId);
|
||
while (1)
|
||
{
|
||
if (a_seed.link[4].id < 0)
|
||
break;
|
||
a_seed = clusterNodeLinks[a_seed.link[4].id];
|
||
|
||
if ((a_seed.link[1].id < 0) && (a_seed.link[2].id < 0) && (a_seed.link[3].id < 0))
|
||
result.push_back(a_seed.objId);
|
||
else
|
||
break;
|
||
}
|
||
return result;
|
||
}
|
||
std::vector<int> _RTseedSearchBottom(NodeOctLink& startNode, std::vector< NodeOctLink>& clusterNodeLinks)
|
||
{
|
||
NodeOctLink a_seed = startNode;
|
||
std::vector<int> result;
|
||
result.push_back(a_seed.objId);
|
||
while (1)
|
||
{
|
||
if (a_seed.link[6].id < 0)
|
||
break;
|
||
a_seed = clusterNodeLinks[a_seed.link[6].id];
|
||
|
||
if ((a_seed.link[1].id < 0) && (a_seed.link[0].id < 0) && (a_seed.link[7].id < 0))
|
||
result.push_back(a_seed.objId);
|
||
else
|
||
break;
|
||
}
|
||
return result;
|
||
}
|
||
int _foundDM_RTcorner(std::vector< NodeOctLink>& clusterNodeLinks, int rows, int cols)
|
||
{
|
||
for (int i = 0, i_max = clusterNodeLinks.size(); i < i_max; i++)
|
||
{
|
||
//寻找种子向右向下
|
||
NodeOctLink a_seed = clusterNodeLinks[i];
|
||
if ((a_seed.link[4].id >= 0) && (a_seed.link[6].id >= 0) &&
|
||
(a_seed.link[0].id < 0) && (a_seed.link[1].id < 0) &&
|
||
(a_seed.link[2].id < 0) && (a_seed.link[3].id < 0) &&
|
||
(a_seed.link[7].id < 0)) //合格的种子
|
||
{
|
||
std::vector<int> L_result = _RTseedSearchLeft(a_seed, clusterNodeLinks);
|
||
std::vector<int> B_result = _RTseedSearchBottom(a_seed, clusterNodeLinks);
|
||
if (((int)L_result.size() == cols) && ((int)B_result.size() == rows)) //找到LT角点
|
||
return i;
|
||
}
|
||
}
|
||
return -1;
|
||
}
|
||
|
||
std::vector<int> _LBseedSearchRight(NodeOctLink& startNode, std::vector< NodeOctLink>& clusterNodeLinks)
|
||
{
|
||
NodeOctLink a_seed = startNode;
|
||
std::vector<int> result;
|
||
result.push_back(a_seed.objId);
|
||
while (1)
|
||
{
|
||
if (a_seed.link[0].id < 0)
|
||
break;
|
||
a_seed = clusterNodeLinks[a_seed.link[0].id];
|
||
|
||
if ((a_seed.link[5].id < 0) && (a_seed.link[6].id < 0) && (a_seed.link[7].id < 0))
|
||
result.push_back(a_seed.objId);
|
||
else
|
||
break;
|
||
}
|
||
return result;
|
||
}
|
||
std::vector<int> _LBseedSearchTop(NodeOctLink& startNode, std::vector< NodeOctLink>& clusterNodeLinks)
|
||
{
|
||
NodeOctLink a_seed = startNode;
|
||
std::vector<int> result;
|
||
result.push_back(a_seed.objId);
|
||
while (1)
|
||
{
|
||
if (a_seed.link[2].id < 0)
|
||
break;
|
||
a_seed = clusterNodeLinks[a_seed.link[2].id];
|
||
|
||
if ((a_seed.link[3].id < 0) && (a_seed.link[4].id < 0) && (a_seed.link[5].id < 0))
|
||
result.push_back(a_seed.objId);
|
||
else
|
||
break;
|
||
}
|
||
return result;
|
||
}
|
||
int _foundDM_LBcorner(std::vector< NodeOctLink>& clusterNodeLinks, int rows, int cols)
|
||
{
|
||
for (int i = 0, i_max = clusterNodeLinks.size(); i < i_max; i++)
|
||
{
|
||
//寻找种子向右向下
|
||
NodeOctLink a_seed = clusterNodeLinks[i];
|
||
if ((a_seed.link[0].id >= 0) && (a_seed.link[2].id >= 0) &&
|
||
(a_seed.link[3].id < 0) && (a_seed.link[4].id < 0) &&
|
||
(a_seed.link[5].id < 0) && (a_seed.link[6].id < 0) &&
|
||
(a_seed.link[7].id < 0)) //合格的种子
|
||
{
|
||
std::vector<int> L_result = _LBseedSearchRight(a_seed, clusterNodeLinks);
|
||
std::vector<int> B_result = _LBseedSearchTop(a_seed, clusterNodeLinks);
|
||
if (((int)L_result.size() == cols) && ((int)B_result.size() == rows)) //找到LT角点
|
||
return i;
|
||
}
|
||
}
|
||
return -1;
|
||
}
|
||
|
||
std::vector<int> _RBseedSearchLeft(NodeOctLink& startNode, std::vector< NodeOctLink>& clusterNodeLinks)
|
||
{
|
||
NodeOctLink a_seed = startNode;
|
||
std::vector<int> result;
|
||
result.push_back(a_seed.objId);
|
||
while (1)
|
||
{
|
||
if (a_seed.link[4].id < 0)
|
||
break;
|
||
a_seed = clusterNodeLinks[a_seed.link[4].id];
|
||
|
||
if ((a_seed.link[5].id < 0) && (a_seed.link[6].id < 0) && (a_seed.link[7].id < 0))
|
||
result.push_back(a_seed.objId);
|
||
else
|
||
break;
|
||
}
|
||
return result;
|
||
}
|
||
std::vector<int> _RBseedSearchTop(NodeOctLink& startNode, std::vector< NodeOctLink>& clusterNodeLinks)
|
||
{
|
||
NodeOctLink a_seed = startNode;
|
||
std::vector<int> result;
|
||
result.push_back(a_seed.objId);
|
||
while (1)
|
||
{
|
||
if (a_seed.link[2].id < 0)
|
||
break;
|
||
a_seed = clusterNodeLinks[a_seed.link[2].id];
|
||
|
||
if ((a_seed.link[1].id < 0) && (a_seed.link[0].id < 0) && (a_seed.link[7].id < 0))
|
||
result.push_back(a_seed.objId);
|
||
else
|
||
break;
|
||
}
|
||
return result;
|
||
}
|
||
int _foundDM_RBcorner(std::vector< NodeOctLink>& clusterNodeLinks, int rows, int cols)
|
||
{
|
||
for (int i = 0, i_max = clusterNodeLinks.size(); i < i_max; i++)
|
||
{
|
||
//寻找种子向右向下
|
||
NodeOctLink a_seed = clusterNodeLinks[i];
|
||
if ((a_seed.ptPos.x > 19.2) && (a_seed.ptPos.y > -31))
|
||
int kkk = 1;
|
||
if ((a_seed.link[2].id >= 0) && (a_seed.link[4].id >= 0) &&
|
||
(a_seed.link[0].id < 0) && (a_seed.link[1].id < 0) &&
|
||
(a_seed.link[5].id < 0) && (a_seed.link[6].id < 0) &&
|
||
(a_seed.link[7].id < 0)) //合格的种子
|
||
{
|
||
std::vector<int> L_result = _RBseedSearchLeft(a_seed, clusterNodeLinks);
|
||
std::vector<int> B_result = _RBseedSearchTop(a_seed, clusterNodeLinks);
|
||
if (((int)L_result.size() == cols) && ((int)B_result.size() == rows)) //找到LT角点
|
||
return i;
|
||
}
|
||
}
|
||
return -1;
|
||
}
|
||
|
||
typedef struct
|
||
{
|
||
int idx;
|
||
SVzNL2DPoint pt2D;
|
||
}DMPos;
|
||
|
||
void _getDMPosLinks(std::vector< DMPos>& posLink, std::vector< NodeOctLink>& clusterNodeLinks)
|
||
{
|
||
int i = 0;
|
||
while (1)
|
||
{
|
||
if (i >= posLink.size())
|
||
break;
|
||
DMPos a_seed = posLink[i];
|
||
NodeOctLink& a_node = clusterNodeLinks[a_seed.idx];
|
||
if (a_node.objId >= 0)
|
||
{
|
||
if (a_node.link[0].id >= 0)
|
||
{
|
||
DMPos new_seed;
|
||
new_seed.idx = a_node.link[0].id;
|
||
new_seed.pt2D = { a_seed.pt2D.x + 1, a_seed.pt2D.y };
|
||
if (clusterNodeLinks[new_seed.idx].objId >= 0)
|
||
{
|
||
clusterNodeLinks[new_seed.idx].link[4].id = -1; //标记已经处理
|
||
posLink.push_back(new_seed);
|
||
}
|
||
}
|
||
if (a_node.link[1].id >= 0)
|
||
{
|
||
DMPos new_seed;
|
||
new_seed.idx = a_node.link[1].id;
|
||
new_seed.pt2D = { a_seed.pt2D.x + 1, a_seed.pt2D.y - 1 };
|
||
if (clusterNodeLinks[new_seed.idx].objId >= 0)
|
||
{
|
||
clusterNodeLinks[new_seed.idx].link[5].id = -1; //标记已经处理
|
||
posLink.push_back(new_seed);
|
||
}
|
||
}
|
||
if (a_node.link[2].id >= 0)
|
||
{
|
||
DMPos new_seed;
|
||
new_seed.idx = a_node.link[2].id;
|
||
new_seed.pt2D = { a_seed.pt2D.x, a_seed.pt2D.y - 1 };
|
||
if (clusterNodeLinks[new_seed.idx].objId >= 0)
|
||
{
|
||
clusterNodeLinks[new_seed.idx].link[6].id = -1; //标记已经处理
|
||
posLink.push_back(new_seed);
|
||
}
|
||
}
|
||
if (a_node.link[3].id >= 0)
|
||
{
|
||
DMPos new_seed;
|
||
new_seed.idx = a_node.link[3].id;
|
||
new_seed.pt2D = { a_seed.pt2D.x - 1, a_seed.pt2D.y - 1 };
|
||
clusterNodeLinks[new_seed.idx].link[7].id = -1; //标记已经处理
|
||
posLink.push_back(new_seed);
|
||
}
|
||
if (a_node.link[4].id >= 0)
|
||
{
|
||
DMPos new_seed;
|
||
new_seed.idx = a_node.link[4].id;
|
||
new_seed.pt2D = { a_seed.pt2D.x - 1, a_seed.pt2D.y };
|
||
if (clusterNodeLinks[new_seed.idx].objId >= 0)
|
||
{
|
||
clusterNodeLinks[new_seed.idx].link[0].id = -1; //标记已经处理
|
||
posLink.push_back(new_seed);
|
||
}
|
||
}
|
||
if (a_node.link[5].id >= 0)
|
||
{
|
||
DMPos new_seed;
|
||
new_seed.idx = a_node.link[5].id;
|
||
new_seed.pt2D = { a_seed.pt2D.x - 1, a_seed.pt2D.y + 1 };
|
||
if (clusterNodeLinks[new_seed.idx].objId >= 0)
|
||
{
|
||
clusterNodeLinks[new_seed.idx].link[1].id = -1; //标记已经处理
|
||
posLink.push_back(new_seed);
|
||
}
|
||
}
|
||
if (a_node.link[6].id >= 0)
|
||
{
|
||
DMPos new_seed;
|
||
new_seed.idx = a_node.link[6].id;
|
||
new_seed.pt2D = { a_seed.pt2D.x, a_seed.pt2D.y + 1 };
|
||
if (clusterNodeLinks[new_seed.idx].objId >= 0)
|
||
{
|
||
clusterNodeLinks[new_seed.idx].link[2].id = -1; //标记已经处理
|
||
posLink.push_back(new_seed);
|
||
}
|
||
}
|
||
if (a_node.link[7].id >= 0)
|
||
{
|
||
DMPos new_seed;
|
||
new_seed.idx = a_node.link[7].id;
|
||
new_seed.pt2D = { a_seed.pt2D.x + 1, a_seed.pt2D.y + 1 };
|
||
if (clusterNodeLinks[new_seed.idx].objId >= 0)
|
||
{
|
||
clusterNodeLinks[new_seed.idx].link[3].id = -1; //标记已经处理
|
||
posLink.push_back(new_seed);
|
||
}
|
||
}
|
||
clusterNodeLinks[a_seed.idx].objId = -1; //标记已经处理
|
||
}
|
||
i++;
|
||
}
|
||
return;
|
||
}
|
||
|
||
SSG_ROIRectD _getClusterROI(std::vector< SVzNL3DPosition>& a_cluster)
|
||
{
|
||
SSG_ROIRectD roi = {0,0,0,0};
|
||
int nodeSize = a_cluster.size();
|
||
if (nodeSize == 0)
|
||
return roi;
|
||
|
||
roi = { a_cluster [0].pt3D.x, a_cluster[0].pt3D.x, a_cluster[0].pt3D.y, a_cluster[0].pt3D.y};
|
||
for (int i = 1; i < nodeSize; i++)
|
||
{
|
||
roi.left = roi.left > a_cluster[i].pt3D.x ? a_cluster[i].pt3D.x : roi.left;
|
||
roi.right = roi.right < a_cluster[i].pt3D.x ? a_cluster[i].pt3D.x : roi.right;
|
||
roi.top = roi.top > a_cluster[i].pt3D.y ? a_cluster[i].pt3D.y : roi.top;
|
||
roi.bottom = roi.bottom < a_cluster[i].pt3D.y ? a_cluster[i].pt3D.y : roi.bottom;
|
||
}
|
||
return roi;
|
||
}
|
||
|
||
void wd_QRcode3Ddetection(
|
||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||
const WD_QRcodeParam qrcode_param,
|
||
std::vector< SVzNL3DPosition>& outObjPoints,
|
||
cv::Mat& dmCodeImg)
|
||
{
|
||
std::vector<std::vector< SSG_basicFeature1D>> lineZMaxPts;
|
||
int lineNum = scanLines.size();
|
||
|
||
double scaleWin = qrcode_param.row_space / 5;
|
||
double minPkHeighth = qrcode_param.pointHoleDepth / 2;
|
||
double holeR = qrcode_param.pointHoleR;
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
std::vector< SSG_basicFeature1D> a_lineMax;
|
||
sg_getFlatLineLocalPeaks_vector(
|
||
scanLines[line],
|
||
line,
|
||
scaleWin,
|
||
minPkHeighth,
|
||
holeR,
|
||
a_lineMax);
|
||
lineZMaxPts.push_back(a_lineMax);
|
||
}
|
||
//特征生长
|
||
SSG_treeGrowParam growParam;
|
||
growParam.yDeviation_max = holeR;//生长时相邻特征最大的Y偏差
|
||
growParam.zDeviation_max = holeR; //生长时相邻特征最大的Z偏差
|
||
growParam.maxLineSkipNum = 5; //生长时相邻特征的最大线间隔， -1时使用maxDkipDistance
|
||
growParam.maxSkipDistance = -1; //若maxLineSkipNum为-1， 使用此参数.设为-1时，此参数无效
|
||
growParam.minLTypeTreeLen = holeR/2; //生长树最少的节点数目。小于此数目的生长树被移除
|
||
growParam.minVTypeTreeLen = holeR/2; //生长树最少的节点数目。小于此数目的生长树被移除
|
||
|
||
std::vector<SSG_featureTree> trees;
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
std::vector< SSG_basicFeature1D>& a_lineMax = lineZMaxPts[line];
|
||
if (a_lineMax.size() == 0)
|
||
continue;
|
||
|
||
//对feature进行生长
|
||
bool isLastLine = false;
|
||
if (line == lineNum - 1)
|
||
isLastLine = true;
|
||
sg_lineFeaturesGrowing(
|
||
line,
|
||
isLastLine,
|
||
a_lineMax,
|
||
trees,
|
||
growParam);
|
||
}
|
||
|
||
//计算目标中心点，以此代表各个点
|
||
std::vector< SVzNL3DPosition> objPoints;
|
||
objPoints.resize(trees.size());
|
||
for (int i = 0, i_max = trees.size(); i < i_max; i++)
|
||
{
|
||
SVzNL3DPosition a_obj;
|
||
a_obj.nPointIdx = i;
|
||
a_obj.pt3D = _getObjCenter(trees[i]);
|
||
objPoints[i] = a_obj;
|
||
}
|
||
//将邻近的点合并
|
||
int objPtSize = objPoints.size();
|
||
for (int i = objPtSize - 1; i >= 0; i--)
|
||
{
|
||
SVzNL3DPosition a_pt = objPoints[i];
|
||
bool isCombined = false;
|
||
for (int j = i - 1; j >= 0; j--)
|
||
{
|
||
SVzNL3DPosition chk_pt = objPoints[j];
|
||
double dist = sqrt(pow(a_pt.pt3D.x - chk_pt.pt3D.x, 2) + pow(a_pt.pt3D.y - chk_pt.pt3D.y, 2));
|
||
if (dist < holeR * 2) //合并
|
||
{
|
||
objPoints[j].pt3D.x = (a_pt.pt3D.x + chk_pt.pt3D.x) / 2;
|
||
objPoints[j].pt3D.y = (a_pt.pt3D.y + chk_pt.pt3D.y) / 2;
|
||
objPoints[j].pt3D.z = (a_pt.pt3D.z + chk_pt.pt3D.z) / 2;
|
||
isCombined = true;
|
||
break;
|
||
}
|
||
}
|
||
if (true == isCombined)
|
||
objPoints.erase(objPoints.begin() + i);
|
||
}
|
||
for (int i = 0, i_max = objPoints.size(); i < i_max; i++)
|
||
outObjPoints.push_back(objPoints[i]);
|
||
|
||
//聚类，分成一个个二维码区域
|
||
double clusterDist = (qrcode_param.row_space + qrcode_param.col_space) * 2; //使用棋盘格距离
|
||
std::vector<std::vector< SVzNL3DPosition>> objClusters;
|
||
sg_pointClustering(
|
||
objPoints,
|
||
clusterDist,
|
||
objClusters //result
|
||
);
|
||
|
||
std::vector<DMCodeInfo> qrCodeResult;
|
||
for (int i = 0, i_max = objClusters.size(); i < i_max; i++)
|
||
{
|
||
std::vector< SVzNL3DPosition>& a_cluster = objClusters[i];
|
||
int clustNodeSize = a_cluster.size();
|
||
//构建相邻关系
|
||
std::vector< NodeOctLink> clusterNodeLinks;
|
||
clusterNodeLinks.resize(clustNodeSize);
|
||
for (int m = 0; m < clustNodeSize; m++) //初始化
|
||
{
|
||
clusterNodeLinks[m].objId = m;
|
||
clusterNodeLinks[m].ptPos = a_cluster[m].pt3D;
|
||
for (int j = 0; j < 8; j++)
|
||
{
|
||
clusterNodeLinks[m].link[j].id = -1;
|
||
clusterNodeLinks[m].link[j].dist = 0.0;
|
||
clusterNodeLinks[m].link[j].angle = 0.0;
|
||
}
|
||
}
|
||
_createNodeLinks(a_cluster, clusterNodeLinks, qrcode_param.row_space, qrcode_param.col_space);
|
||
//寻找DM二维码特征
|
||
int cornerIdx = 0;
|
||
int cornerNodeIdx = _foundDM_LTcorner(clusterNodeLinks, qrcode_param.rows, qrcode_param.cols);
|
||
if (cornerNodeIdx < 0)
|
||
{
|
||
cornerIdx = 1;
|
||
cornerNodeIdx = _foundDM_RTcorner(clusterNodeLinks, qrcode_param.rows, qrcode_param.cols);
|
||
if (cornerNodeIdx < 0)
|
||
{
|
||
cornerIdx = 2;
|
||
cornerNodeIdx = _foundDM_LBcorner(clusterNodeLinks, qrcode_param.rows, qrcode_param.cols);
|
||
if (cornerNodeIdx < 0)
|
||
{
|
||
cornerIdx = 3;
|
||
cornerNodeIdx = _foundDM_RBcorner(clusterNodeLinks, qrcode_param.rows, qrcode_param.cols);
|
||
}
|
||
}
|
||
}
|
||
if (cornerNodeIdx >= 0)
|
||
{
|
||
//提取二维码编码值
|
||
int rows = qrcode_param.rows;
|
||
int cols = qrcode_param.cols;
|
||
|
||
DMPos startPos;
|
||
startPos.idx = cornerNodeIdx;
|
||
if (cornerIdx == 0) //LT
|
||
startPos.pt2D = { 0,0 };
|
||
else if (cornerIdx == 1) //RT
|
||
startPos.pt2D = { 0,cols-1 };
|
||
else if(cornerIdx == 2) //LB
|
||
startPos.pt2D = { rows-1,0 };
|
||
else //RB
|
||
startPos.pt2D = { rows - 1, cols-1 };
|
||
|
||
std::vector< DMPos> posLinks;
|
||
posLinks.push_back(startPos);
|
||
_getDMPosLinks(posLinks, clusterNodeLinks);
|
||
|
||
int* codeBuff = (int*)malloc(sizeof(int) * rows * cols);
|
||
memset(codeBuff, 0, sizeof(int) * qrcode_param.rows * qrcode_param.cols);
|
||
for (int m = 0, m_max = posLinks.size(); m < m_max; m++)
|
||
{
|
||
int col = posLinks[m].pt2D.x;
|
||
int row = posLinks[m].pt2D.y;
|
||
codeBuff[row * cols + col] = 1;
|
||
}
|
||
//
|
||
DMCodeInfo a_dmCode;
|
||
a_dmCode.roi = _getClusterROI(a_cluster);
|
||
a_dmCode.qrCode = codeBuff;
|
||
qrCodeResult.push_back(a_dmCode);
|
||
}
|
||
}
|
||
if (qrCodeResult.size() > 0)
|
||
{
|
||
SVzNL3DRangeD globalRoi = sg_getScanDataROI_vector(scanLines);
|
||
//恢复二维码编码图像
|
||
int dmNodeSize = 60;
|
||
int dmLen = dmNodeSize * qrcode_param.cols;
|
||
double scale = (qrCodeResult[0].roi.right - qrCodeResult[0].roi.left) / (double)dmLen;
|
||
int skip = dmNodeSize;
|
||
int img_cols = (int)((globalRoi.xRange.max - globalRoi.xRange.min) / scale);
|
||
img_cols = ((img_cols + 3) / 4) * 4; //4对齐
|
||
int img_rows = (int)((globalRoi.yRange.max - globalRoi.yRange.min) / scale);
|
||
img_rows = ((img_rows + 3) / 4) * 4; //4对齐
|
||
//加上空白边
|
||
img_cols += skip * 2;
|
||
img_rows += skip * 2;
|
||
dmCodeImg = cv::Mat::ones(img_rows, img_cols, CV_8UC1);
|
||
for (int i = 0; i < qrCodeResult.size(); i++)
|
||
{
|
||
double x = qrCodeResult[i].roi.left;
|
||
double y = qrCodeResult[i].roi.top;
|
||
SVzNL2DPoint cornerPos;
|
||
cornerPos.x = (int)((x-globalRoi.xRange.min) / scale) + skip;
|
||
cornerPos.y = (int)((y-globalRoi.yRange.min) / scale) + skip;
|
||
for (int row = 0; row < qrcode_param.rows; row++)
|
||
{
|
||
for (int col = 0; col < qrcode_param.cols; col++)
|
||
{
|
||
SVzNL2DPoint nodePos;
|
||
nodePos.x = cornerPos.x + col * dmNodeSize;
|
||
nodePos.y = cornerPos.y + row * dmNodeSize;
|
||
if (qrCodeResult[i].qrCode[row * qrcode_param.cols + col] > 0)
|
||
{
|
||
for (int m = -dmNodeSize / 2; m < dmNodeSize / 2; m++)
|
||
{
|
||
for (int n = -dmNodeSize / 2; n < dmNodeSize / 2; n++)
|
||
dmCodeImg.at<uchar>(nodePos.y + m, nodePos.x + n) = 0;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
free(qrCodeResult[i].qrCode);
|
||
qrCodeResult[i].qrCode = NULL;
|
||
}
|
||
}
|
||
return;
|
||
} |