297 lines
7.9 KiB
C++
297 lines
7.9 KiB
C++
|
#include <vector>
|
|||
|
|
#include "SG_baseDataType.h"
|
|||
|
|
#include "SG_baseAlgo_Export.h"
|
|||
|
|
#include "WD_glovePositioning_Export.h"
|
|||
|
|
#include <opencv2/opencv.hpp>
|
|||
|
|
#include <limits>
|
|||
|
|
|
|||
|
|
//<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)
|
|||
|
|
{
|
|||
|
|
std::vector<SVzNL3DPosition> a_line;
|
|||
|
|
for (int i = 0, i_max = scanData.size(); i < i_max; i++)
|
|||
|
|
{
|
|||
|
|
SVzNL3DPoint a_pt = scanData[i];
|
|||
|
|
SVzNL3DPosition a_idxPt;
|
|||
|
|
a_idxPt.nPointIdx = 0;
|
|||
|
|
a_idxPt.pt3D = a_pt;
|
|||
|
|
if (a_line.size() == 0)
|
|||
|
|
a_line.push_back(a_idxPt);
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
SVzNL3DPosition pre_pt = a_line.back();
|
|||
|
|
double x_diff = abs(pre_pt.pt3D.x - a_idxPt.pt3D.x);
|
|||
|
|
if (x_diff > 1e-4)
|
|||
|
|
{
|
|||
|
|
scanLines.push_back(a_line);
|
|||
|
|
a_line.clear();
|
|||
|
|
a_line.push_back(a_idxPt);
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
a_line.push_back(a_idxPt);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
//last line
|
|||
|
|
if(a_line.size() > 0)
|
|||
|
|
scanLines.push_back(a_line);
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void wd_lineDataR(std::vector< SVzNL3DPosition>& a_line,
|
|||
|
|
const double* camPoseR,
|
|||
|
|
double groundH)
|
|||
|
|
{
|
|||
|
|
lineDataRT_vector(a_line, camPoseR, groundH);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void wd_getGloveGrabPostion(
|
|||
|
|
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
|||
|
|
const SSG_gloveArcParam arcPara,
|
|||
|
|
SSG_treeGrowParam growParam,
|
|||
|
|
std::vector<SSG_peakRgnInfo>& objOps,
|
|||
|
|
int* errCode)
|
|||
|
|
{
|
|||
|
|
*errCode = 0;
|
|||
|
|
int lineNum = (int)scanLines.size();
|
|||
|
|
if (lineNum == 0)
|
|||
|
|
{
|
|||
|
|
*errCode = SG_ERR_3D_DATA_NULL;
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
int linePtNum = (int)scanLines[0].size();
|
|||
|
|
bool isGridData = true;
|
|||
|
|
//<2F><>ֱarc<72><63><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ
|
|||
|
|
std::vector<std::vector<SSG_basicFeature1D>> arcFeatures;
|
|||
|
|
for(int line = 0; line < lineNum; line ++)
|
|||
|
|
{
|
|||
|
|
std::vector<SSG_basicFeature1D> lineArcFeatures;
|
|||
|
|
std::vector<SVzNL3DPosition>& lineData = scanLines[line];
|
|||
|
|
if (linePtNum != (int)lineData.size())
|
|||
|
|
isGridData = false;
|
|||
|
|
|
|||
|
|
wd_getLineGloveArcs(
|
|||
|
|
lineData,
|
|||
|
|
line,
|
|||
|
|
arcPara,
|
|||
|
|
lineArcFeatures);
|
|||
|
|
arcFeatures.push_back(lineArcFeatures);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if (false == isGridData)//<2F><><EFBFBD>ݲ<EFBFBD><DDB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʽ
|
|||
|
|
{
|
|||
|
|
*errCode = SG_ERR_NOT_GRID_FORMAT;
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD><EFBFBD>ˮƽɨ<C6BD><C9A8>
|
|||
|
|
std::vector<std::vector<SVzNL3DPosition>> hLines;
|
|||
|
|
hLines.resize(linePtNum);
|
|||
|
|
for (int i = 0; i < linePtNum; i++)
|
|||
|
|
hLines[i].resize(lineNum);
|
|||
|
|
for (int line = 0; line < lineNum; line++)
|
|||
|
|
{
|
|||
|
|
for (int j = 0; j < linePtNum; j++)
|
|||
|
|
{
|
|||
|
|
scanLines[line][j].nPointIdx = 0; //<2F><>ԭʼ<D4AD><CABC><EFBFBD>ݵ<EFBFBD><DDB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0<EFBFBD><30><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>ʹ<EFBFBD>ã<EFBFBD>
|
|||
|
|
hLines[j][line] = scanLines[line][j];
|
|||
|
|
hLines[j][line].pt3D.x = scanLines[line][j].pt3D.y;
|
|||
|
|
hLines[j][line].pt3D.y = scanLines[line][j].pt3D.x;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
//ˮƽarc<72><63><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ
|
|||
|
|
std::vector<std::vector<SSG_basicFeature1D>> arcFeatures_h;
|
|||
|
|
int lineNum_h = (int)hLines.size();
|
|||
|
|
for (int line = 0; line < lineNum_h; line++)
|
|||
|
|
{
|
|||
|
|
std::vector<SSG_basicFeature1D> lineArcFeatures;
|
|||
|
|
std::vector<SVzNL3DPosition>& lineData = hLines[line];
|
|||
|
|
wd_getLineGloveArcs(
|
|||
|
|
lineData,
|
|||
|
|
line,
|
|||
|
|
arcPara,
|
|||
|
|
lineArcFeatures);
|
|||
|
|
arcFeatures_h.push_back(lineArcFeatures);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
//<2F><>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>߷<EFBFBD><DFB7><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
std::vector<SSG_featureTree> v_trees;
|
|||
|
|
for (int line = 0; line < lineNum; line++)
|
|||
|
|
{
|
|||
|
|
bool isLastLine = false;
|
|||
|
|
if (line == lineNum - 1)
|
|||
|
|
isLastLine = true;
|
|||
|
|
std::vector<SSG_basicFeature1D>& a_lineArcFeature = arcFeatures[line];
|
|||
|
|
sg_lineFeaturesGrowing(
|
|||
|
|
line,
|
|||
|
|
isLastLine,
|
|||
|
|
a_lineArcFeature,
|
|||
|
|
v_trees,
|
|||
|
|
growParam);
|
|||
|
|
}
|
|||
|
|
//ˮƽ<CBAE><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɨ<EFBFBD><C9A8><EFBFBD>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
std::vector<SSG_featureTree> h_trees;
|
|||
|
|
for (int line = 0; line < lineNum_h; line++)
|
|||
|
|
{
|
|||
|
|
if (line == 650)
|
|||
|
|
int kkk = 1;
|
|||
|
|
bool isLastLine = false;
|
|||
|
|
if (line == lineNum_h - 1)
|
|||
|
|
isLastLine = true;
|
|||
|
|
std::vector<SSG_basicFeature1D>& a_lineArcFeature = arcFeatures_h[line];
|
|||
|
|
sg_lineFeaturesGrowing(
|
|||
|
|
line,
|
|||
|
|
isLastLine,
|
|||
|
|
a_lineArcFeature,
|
|||
|
|
h_trees,
|
|||
|
|
growParam);
|
|||
|
|
}
|
|||
|
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD>ת<EFBFBD>ǵ<EFBFBD><C7B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
double maxTurn_v = 0;
|
|||
|
|
int maxTurn_v_id = -1;
|
|||
|
|
for (int i = 0, i_max = (int)v_trees.size(); i < i_max; i++)
|
|||
|
|
{
|
|||
|
|
int nodeSize = (int)v_trees[i].treeNodes.size();
|
|||
|
|
v_trees[i].tree_value = v_trees[i].tree_value / (double)nodeSize;
|
|||
|
|
if (maxTurn_v_id < 0)
|
|||
|
|
{
|
|||
|
|
maxTurn_v_id = i;
|
|||
|
|
maxTurn_v = v_trees[i].tree_value;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
if (maxTurn_v < v_trees[i].tree_value)
|
|||
|
|
{
|
|||
|
|
maxTurn_v_id = i;
|
|||
|
|
maxTurn_v = v_trees[i].tree_value;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
double maxTurn_h = 0;
|
|||
|
|
int maxTurn_h_id = -1;
|
|||
|
|
for (int i = 0, i_max = (int)h_trees.size(); i < i_max; i++)
|
|||
|
|
{
|
|||
|
|
int nodeSize = (int)h_trees[i].treeNodes.size();
|
|||
|
|
h_trees[i].tree_value = h_trees[i].tree_value / (double)nodeSize;
|
|||
|
|
if (maxTurn_h_id < 0)
|
|||
|
|
{
|
|||
|
|
maxTurn_h_id = i;
|
|||
|
|
maxTurn_h = h_trees[i].tree_value;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
if (maxTurn_h < h_trees[i].tree_value)
|
|||
|
|
{
|
|||
|
|
maxTurn_h_id = i;
|
|||
|
|
maxTurn_h = h_trees[i].tree_value;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
if ((maxTurn_v_id >= 0) && (maxTurn_h_id < 0))
|
|||
|
|
{
|
|||
|
|
SSG_featureTree bestTree = v_trees[maxTurn_v_id];
|
|||
|
|
v_trees.clear();
|
|||
|
|
v_trees.push_back(bestTree);
|
|||
|
|
}
|
|||
|
|
else if ((maxTurn_v_id < 0) && (maxTurn_h_id >= 0))
|
|||
|
|
{
|
|||
|
|
SSG_featureTree bestTree = h_trees[maxTurn_h_id];
|
|||
|
|
h_trees.clear();
|
|||
|
|
h_trees.push_back(bestTree);
|
|||
|
|
}
|
|||
|
|
else if ((maxTurn_v_id >= 0) && (maxTurn_h_id >= 0))
|
|||
|
|
{
|
|||
|
|
if (maxTurn_h <= maxTurn_v)
|
|||
|
|
{
|
|||
|
|
h_trees.clear();
|
|||
|
|
SSG_featureTree bestTree = v_trees[maxTurn_v_id];
|
|||
|
|
v_trees.clear();
|
|||
|
|
v_trees.push_back(bestTree);
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
v_trees.clear();
|
|||
|
|
SSG_featureTree bestTree = h_trees[maxTurn_h_id];
|
|||
|
|
h_trees.clear();
|
|||
|
|
h_trees.push_back(bestTree);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//tree<65><65>Ϣ
|
|||
|
|
std::vector<SSG_treeInfo> allTreesInfo; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>߽<EFBFBD>
|
|||
|
|
SSG_treeInfo a_nullTree;
|
|||
|
|
memset(&a_nullTree, 0, sizeof(SSG_treeInfo));
|
|||
|
|
allTreesInfo.push_back(a_nullTree); //<2F><><EFBFBD>ִ洢λ<E6B4A2><CEBB><EFBFBD><EFBFBD>treeIdx<64><78>ͬλ<CDAC>ã<EFBFBD><C3A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
//<2F><><EFBFBD>ǣ<EFBFBD><C7A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>б<EFBFBD>ע
|
|||
|
|
int hvTreeIdx = 1;
|
|||
|
|
for (int i = 0, i_max = (int)v_trees.size(); i < i_max; i++)
|
|||
|
|
{
|
|||
|
|
SSG_featureTree* a_vTree = &v_trees[i];
|
|||
|
|
|
|||
|
|
//<2F><>¼Tree<65><65><EFBFBD><EFBFBD>Ϣ
|
|||
|
|
SSG_treeInfo a_treeInfo;
|
|||
|
|
a_treeInfo.vTreeFlag = 1;
|
|||
|
|
a_treeInfo.treeIdx = hvTreeIdx;
|
|||
|
|
a_treeInfo.treeType = a_vTree->treeType;
|
|||
|
|
a_treeInfo.sLineIdx = a_vTree->sLineIdx;
|
|||
|
|
a_treeInfo.eLineIdx = a_vTree->eLineIdx;
|
|||
|
|
a_treeInfo.roi = a_vTree->roi;
|
|||
|
|
allTreesInfo.push_back(a_treeInfo);
|
|||
|
|
//<2F><>ԭʼ<D4AD><CABC><EFBFBD><EFBFBD><EFBFBD>ϱ<EFBFBD><CFB1>ǣ<EFBFBD>ͬʱ<CDAC><CAB1>Mask<73>ϱ<EFBFBD><CFB1><EFBFBD>
|
|||
|
|
for (int j = 0, j_max = (int)a_vTree->treeNodes.size(); j < j_max; j++)
|
|||
|
|
{
|
|||
|
|
SSG_basicFeature1D* a_feature = &a_vTree->treeNodes[j];
|
|||
|
|
if (scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].pt3D.z > 1e-4)//<2F><><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD>˺<EFBFBD><CBBA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0
|
|||
|
|
{
|
|||
|
|
int existEdgeId = scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx >> 16;
|
|||
|
|
if (existEdgeId == 0)
|
|||
|
|
{
|
|||
|
|
scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx = a_feature->featureType;
|
|||
|
|
scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx &= 0xffff;
|
|||
|
|
scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].nPointIdx += hvTreeIdx << 16;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
hvTreeIdx++;
|
|||
|
|
}
|
|||
|
|
int hTreeStart = hvTreeIdx;
|
|||
|
|
////<2F><>ע:ˮƽ<CBAE><C6BD><EFBFBD><EFBFBD>
|
|||
|
|
for (int i = 0, i_max = (int)h_trees.size(); i < i_max; i++)
|
|||
|
|
{
|
|||
|
|
SSG_featureTree* a_hTree = &h_trees[i];
|
|||
|
|
//<2F><>¼Tree<65><65><EFBFBD><EFBFBD>Ϣ
|
|||
|
|
SSG_treeInfo a_treeInfo;
|
|||
|
|
a_treeInfo.vTreeFlag = 0;
|
|||
|
|
a_treeInfo.treeIdx = hvTreeIdx;
|
|||
|
|
a_treeInfo.treeType = a_hTree->treeType;
|
|||
|
|
a_treeInfo.sLineIdx = a_hTree->sLineIdx;
|
|||
|
|
a_treeInfo.eLineIdx = a_hTree->eLineIdx;
|
|||
|
|
a_treeInfo.roi.left = a_hTree->roi.top; //ˮƽɨ<C6BD><C9A8>xy<78>ǽ<EFBFBD><C7BD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
a_treeInfo.roi.right = a_hTree->roi.bottom;
|
|||
|
|
a_treeInfo.roi.top = a_hTree->roi.left;
|
|||
|
|
a_treeInfo.roi.bottom = a_hTree->roi.right;
|
|||
|
|
allTreesInfo.push_back(a_treeInfo);
|
|||
|
|
//<2F><>ԭʼ<D4AD><CABC><EFBFBD><EFBFBD><EFBFBD>ϱ<EFBFBD><CFB1>ǣ<EFBFBD>ͬʱ<CDAC><CAB1>Mask<73>ϱ<EFBFBD><CFB1><EFBFBD>
|
|||
|
|
for (int j = 0, j_max = (int)a_hTree->treeNodes.size(); j < j_max; j++)
|
|||
|
|
{
|
|||
|
|
SSG_basicFeature1D* a_feature = &a_hTree->treeNodes[j];
|
|||
|
|
if (scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].pt3D.z > 1e-4)//<2F><><EFBFBD><EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD>˺<EFBFBD><CBBA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0
|
|||
|
|
{
|
|||
|
|
int existEdgeId = scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx >> 16;
|
|||
|
|
if (existEdgeId == 0)
|
|||
|
|
{
|
|||
|
|
scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx += a_feature->featureType << 4;
|
|||
|
|
scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx &= 0xffff;
|
|||
|
|
scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].nPointIdx += hvTreeIdx << 16;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
hvTreeIdx++;
|
|||
|
|
}
|
|||
|
|
int hvTreeSize = hvTreeIdx;
|
|||
|
|
|
|||
|
|
|
|||
|
|
}
|