jerryzeng/algoLib
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
algoLib/sourceCode/WD_QRcode3Ddetection.cpp

811 lines
22 KiB
C++
Raw Normal View History

QRCode 3D detection algorithm initial check in
2025-07-15 21:37:09 +08:00
#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;
//<2F><>ply<6C><79>ʽ<EFBFBD><CABD><EFBFBD><EFBFBD><EFBFBD>ݻָ<DDBB><D6B8><EFBFBD>ɨ<EFBFBD><C9A8><EFBFBD>е<EFBFBD><D0B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʽ<EFBFBD><CABD><EFBFBD>Ӷ<EFBFBD><D3B6><EFBFBD><EFBFBD>水<EFBFBD>н<EFBFBD><D0BD>д<EFBFBD><D0B4><EFBFBD>
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)
{
//<2F>µ<EFBFBD>һ<EFBFBD><D2BB>
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;
}
//<2F><><EFBFBD><EFBFBD>Node<64><65><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ҵ<EFBFBD>Link<6E><6B>ϵ
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) //<2F>ֹ<EFBFBD><D6B9><EFBFBD>
{
double angle = atan2(a_node.pt3D.y - chk_node.pt3D.y, chk_node.pt3D.x - a_node.pt3D.x); //ͼ<><CDBC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵy<CFB5><79><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ŷ<EFBFBD><C5B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5>ô
angle = angle * 180.0 / PI; //תΪ<D7AA>Ƕ<EFBFBD>
if (angle < 0)
angle += 360; //ת<><D7AA>0-360<36><30>
_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++)
{
//Ѱ<><D1B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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)) //<2F>ϸ<EFBFBD><CFB8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
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)) //<2F>ҵ<EFBFBD>LT<4C>ǵ<EFBFBD>
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++)
{
//Ѱ<><D1B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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)) //<2F>ϸ<EFBFBD><CFB8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
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)) //<2F>ҵ<EFBFBD>LT<4C>ǵ<EFBFBD>
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++)
{
//Ѱ<><D1B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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)) //<2F>ϸ<EFBFBD><CFB8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
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)) //<2F>ҵ<EFBFBD>LT<4C>ǵ<EFBFBD>
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++)
{
//Ѱ<><D1B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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)) //<2F>ϸ<EFBFBD><CFB8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
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)) //<2F>ҵ<EFBFBD>LT<4C>ǵ<EFBFBD>
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; //<2F><><EFBFBD><EFBFBD><EFBFBD>Ѿ<EFBFBD><D1BE><EFBFBD><EFBFBD><EFBFBD>
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; //<2F><><EFBFBD><EFBFBD><EFBFBD>Ѿ<EFBFBD><D1BE><EFBFBD><EFBFBD><EFBFBD>
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; //<2F><><EFBFBD><EFBFBD><EFBFBD>Ѿ<EFBFBD><D1BE><EFBFBD><EFBFBD><EFBFBD>
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; //<2F><><EFBFBD><EFBFBD><EFBFBD>Ѿ<EFBFBD><D1BE><EFBFBD><EFBFBD><EFBFBD>
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; //<2F><><EFBFBD><EFBFBD><EFBFBD>Ѿ<EFBFBD><D1BE><EFBFBD><EFBFBD><EFBFBD>
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; //<2F><><EFBFBD><EFBFBD><EFBFBD>Ѿ<EFBFBD><D1BE><EFBFBD><EFBFBD><EFBFBD>
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; //<2F><><EFBFBD><EFBFBD><EFBFBD>Ѿ<EFBFBD><D1BE><EFBFBD><EFBFBD><EFBFBD>
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; //<2F><><EFBFBD><EFBFBD><EFBFBD>Ѿ<EFBFBD><D1BE><EFBFBD><EFBFBD><EFBFBD>
posLink.push_back(new_seed);
}
}
clusterNodeLinks[a_seed.idx].objId = -1; //<2F><><EFBFBD><EFBFBD><EFBFBD>Ѿ<EFBFBD><D1BE><EFBFBD><EFBFBD><EFBFBD>
}
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);
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
SSG_treeGrowParam growParam;
growParam.yDeviation_max = holeR;//<2F><><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Yƫ<59><C6AB>
growParam.zDeviation_max = holeR; //<2F><><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Zƫ<5A><C6AB>
growParam.maxLineSkipNum = 5; //<2F><><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>߼<EFBFBD><DFBC><EFBFBD><EFBFBD><EFBFBD> -1ʱʹ<CAB1><CAB9>maxDkipDistance
growParam.maxSkipDistance = -1; //<2F><>maxLineSkipNumΪ-1<><31> ʹ<>ô˲<C3B4><CBB2><EFBFBD>.<2E><>Ϊ-1ʱ<31><CAB1><EFBFBD>˲<EFBFBD><CBB2><EFBFBD><EFBFBD><EFBFBD>Ч
growParam.minLTypeTreeLen = holeR/2; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٵĽڵ<C4BD><DAB5><EFBFBD>Ŀ<EFBFBD><C4BF>С<EFBFBD>ڴ<EFBFBD><DAB4><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƴ<EFBFBD>
growParam.minVTypeTreeLen = holeR/2; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٵĽڵ<C4BD><DAB5><EFBFBD>Ŀ<EFBFBD><C4BF>С<EFBFBD>ڴ<EFBFBD><DAB4><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƴ<EFBFBD>
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;
//<2F><>feature<72><65><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
bool isLastLine = false;
if (line == lineNum - 1)
isLastLine = true;
sg_lineFeaturesGrowing(
line,
isLastLine,
a_lineMax,
trees,
growParam);
}
//<2F><><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD>ĵ㣬<C4B5>Դ˴<D4B4><CBB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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;
}
//<2F><><EFBFBD>ڽ<EFBFBD><DABD>ĵ<EFBFBD><C4B5>ϲ<EFBFBD>
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) //<2F>ϲ<EFBFBD>
{
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]);
//<2F><><EFBFBD>࣬<EFBFBD>ֳ<EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ά<EFBFBD><CEAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
double clusterDist = (qrcode_param.row_space + qrcode_param.col_space) * 2; //ʹ<><CAB9><EFBFBD><EFBFBD><EFBFBD≯<EFBFBD><CCB8><EFBFBD><EFBFBD><EFBFBD>
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();
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڹ<EFBFBD>ϵ
std::vector< NodeOctLink> clusterNodeLinks;
clusterNodeLinks.resize(clustNodeSize);
for (int m = 0; m < clustNodeSize; m++) //<2F><>ʼ<EFBFBD><CABC>
{
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);
//Ѱ<><D1B0>DM<44><4D>ά<EFBFBD><CEAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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)
{
//<2F><>ȡ<EFBFBD><C8A1>ά<EFBFBD><CEAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
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);
//<2F>ָ<EFBFBD><D6B8><EFBFBD>ά<EFBFBD><CEAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͼ<EFBFBD><CDBC>
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<><34><EFBFBD><EFBFBD>
int img_rows = (int)((globalRoi.yRange.max - globalRoi.yRange.min) / scale);
img_rows = ((img_rows + 3) / 4) * 4; //4<><34><EFBFBD><EFBFBD>
//<2F><><EFBFBD>Ͽհױ<D5B0>
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;
}
Reference in New Issue Copy Permalink