QRCode 3D detection algorithm initial check in
This commit is contained in:
parent
71e8f12480
commit
ae46895573
67
sourceCode/SG_clustering.cpp
Normal file
67
sourceCode/SG_clustering.cpp
Normal file
@ -0,0 +1,67 @@
|
||||
#include "SG_baseDataType.h"
|
||||
#include "SG_baseAlgo_Export.h"
|
||||
#include <vector>
|
||||
#include <corecrt_math_defines.h>
|
||||
#include <cmath>
|
||||
|
||||
void _seedClustering(
|
||||
std::vector< SVzNL3DPosition>& a_cluster,
|
||||
std::vector< SVzNL3DPosition>& pts,
|
||||
double clusterDist)
|
||||
{
|
||||
int i = 0;
|
||||
while (1)
|
||||
{
|
||||
if (i >= a_cluster.size())
|
||||
break;
|
||||
|
||||
SVzNL3DPosition a_seed = a_cluster[i];
|
||||
for (int i = 0, i_max = pts.size(); i < i_max; i++)
|
||||
{
|
||||
if (pts[i].nPointIdx < 0)
|
||||
continue;
|
||||
double dist = sqrt(pow(a_seed.pt3D.x - pts[i].pt3D.x, 2) + pow(a_seed.pt3D.y - pts[i].pt3D.y, 2));
|
||||
if (dist < clusterDist)
|
||||
{
|
||||
a_cluster.push_back(pts[i]);
|
||||
pts[i].nPointIdx = -1;
|
||||
}
|
||||
}
|
||||
i++;
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
void sg_pointClustering(
|
||||
std::vector< SVzNL3DPosition>& pts,
|
||||
double clusterDist,
|
||||
std::vector<std::vector< SVzNL3DPosition>>& objClusters //result
|
||||
)
|
||||
{
|
||||
int ptSize = pts.size();
|
||||
if (ptSize == 0)
|
||||
return;
|
||||
while(pts.size() > 0)
|
||||
{
|
||||
SVzNL3DPosition a_pt = pts[0];
|
||||
|
||||
//新建一个cluster
|
||||
std::vector< SVzNL3DPosition> a_cluster;
|
||||
a_cluster.push_back(a_pt);
|
||||
pts[0].nPointIdx = -1; //防止重复被计算
|
||||
_seedClustering(
|
||||
a_cluster,
|
||||
pts,
|
||||
clusterDist);
|
||||
objClusters.push_back(a_cluster); //保存一个聚类
|
||||
//将pts中处理过的点去除
|
||||
int m_max = pts.size();
|
||||
for (int m = m_max - 1; m >= 0; m--) //从后往前，这样删除不会影响循环
|
||||
{
|
||||
if(pts[m].nPointIdx < 0)
|
||||
pts.erase(pts.begin() + m);
|
||||
}
|
||||
}
|
||||
return;
|
||||
}
|
||||
;
|
||||
811
sourceCode/WD_QRcode3Ddetection.cpp
Normal file
811
sourceCode/WD_QRcode3Ddetection.cpp
Normal file
@ -0,0 +1,811 @@
|
||||
#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;
|
||||
}
|
||||
42
sourceCode/WD_QRcode3Ddetection_Export.h
Normal file
42
sourceCode/WD_QRcode3Ddetection_Export.h
Normal file
@ -0,0 +1,42 @@
|
||||
#pragma once
|
||||
|
||||
#if defined(SG_API_LIBRARY)
|
||||
# define SG_APISHARED_EXPORT __declspec(dllexport)
|
||||
#else
|
||||
# define SG_APISHARED_EXPORT __declspec(dllimport)
|
||||
#endif
|
||||
|
||||
#include "SG_baseDataType.h"
|
||||
#include <vector>
|
||||
#include <opencv2/opencv.hpp>
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int rows; //二维码行数
|
||||
int cols; //二维码列数
|
||||
double row_space; //行间距，单位mm
|
||||
double col_space; //列间距，单位mm
|
||||
double pointHoleDepth; //二维码点的深度，单位mm
|
||||
double pointHoleR; //二维码点的半径，单位mm
|
||||
}WD_QRcodeParam;
|
||||
|
||||
//将ply格式的数据恢复成扫描行的数据形式，从而方面按行进行处理
|
||||
SG_APISHARED_EXPORT void wd_getScanLines(
|
||||
std::vector<SVzNL3DPoint>& scanData,
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||||
int scan_rows);
|
||||
|
||||
SG_APISHARED_EXPORT SSG_planeCalibPara wd_getBaseCalibPara(
|
||||
SVzNL3DLaserLine* laser3DPoints,
|
||||
int lineNum);
|
||||
|
||||
SG_APISHARED_EXPORT void wd_lineDataR(
|
||||
std::vector< SVzNL3DPosition>& a_line,
|
||||
const double* camPoseR,
|
||||
double groundH);
|
||||
|
||||
SG_APISHARED_EXPORT void wd_QRcode3Ddetection(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||||
const WD_QRcodeParam qrcode_param,
|
||||
std::vector< SVzNL3DPosition>& objPoints,
|
||||
cv::Mat& dmCodeImg);
|
||||
Loading…
x
Reference in New Issue
Block a user