1105 lines
35 KiB
C++
1105 lines
35 KiB
C++
#include <vector>
|
||
#include "SG_baseDataType.h"
|
||
#include "SG_baseAlgo_Export.h"
|
||
#include "WD_particleSizeMeasure_Export.h"
|
||
#include <opencv2/opencv.hpp>
|
||
#include <limits>
|
||
|
||
//计算一个平面调平参数。
|
||
//数据输入中可以有一个地平面和参考调平平面，以最高的平面进行调平
|
||
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
|
||
SSG_planeCalibPara wd_getBaseCalibPara(
|
||
std::vector< std::vector<SVzNL3DPosition>>& scanLines)
|
||
{
|
||
return sg_getPlaneCalibPara2(scanLines);
|
||
}
|
||
|
||
//相机姿态调平，并去除地面
|
||
void wd_lineDataR(
|
||
std::vector< SVzNL3DPosition>& a_line,
|
||
const double* camPoseR,
|
||
double groundH)
|
||
{
|
||
lineDataRT_vector(a_line, camPoseR, groundH);
|
||
}
|
||
|
||
void wd_noiseFilter(
|
||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||
const SWD_PSM_algoParam algoParam,
|
||
int* errCode)
|
||
{
|
||
*errCode = 0;
|
||
int lineNum = (int)scanLines.size();
|
||
int nPointCnt = (int)scanLines[0].size();
|
||
bool vldGrid = true;
|
||
for (int i = 0; i < lineNum; i++)
|
||
{
|
||
if (nPointCnt != (int)scanLines[i].size())
|
||
vldGrid = false;
|
||
wd_vectorDataRemoveOutlier_overwrite(
|
||
scanLines[i],
|
||
algoParam.filterParam);
|
||
}
|
||
if (false == vldGrid)
|
||
{
|
||
*errCode = SG_ERR_3D_DATA_INVLD;
|
||
return;
|
||
}
|
||
//水平方向过滤
|
||
int hLineNum = nPointCnt; //Grid格式，所有扫描线的点数是一样的
|
||
//生成水平扫描数据
|
||
std::vector<std::vector<SVzNL3DPosition>> filterHLines;
|
||
filterHLines.resize(hLineNum);
|
||
for (int i = 0; i < hLineNum; i++)
|
||
filterHLines[i].resize(lineNum);
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
for (int j = 0; j < hLineNum; j++)
|
||
{
|
||
filterHLines[j][line] = scanLines[line][j];
|
||
filterHLines[j][line].pt3D.x = scanLines[line][j].pt3D.y;
|
||
filterHLines[j][line].pt3D.y = scanLines[line][j].pt3D.x;
|
||
}
|
||
}
|
||
for (int hLine = 0; hLine < hLineNum; hLine++)
|
||
{
|
||
//滤波，滤除异常点
|
||
std::vector<SVzNL3DPosition> filterData;
|
||
std::vector<int> lineNoise;
|
||
sg_lineDataRemoveOutlier(
|
||
(SVzNL3DPosition*)filterHLines[hLine].data(),
|
||
(int)filterHLines[hLine].size(),
|
||
algoParam.filterParam,
|
||
filterData,
|
||
lineNoise);
|
||
for (int j = 0; j < lineNoise.size(); j++)
|
||
{
|
||
int lineIdx = lineNoise[j];
|
||
scanLines[lineIdx][hLine].pt3D.z = 0;
|
||
}
|
||
}
|
||
return;
|
||
}
|
||
|
||
//粒径检测
|
||
void wd_particleSizeMeasure(
|
||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||
const SWD_paricleSizeParam particleSizeParam,
|
||
const SSG_planeCalibPara groundCalibPara,
|
||
const SWD_PSM_algoParam algoParam,
|
||
std::vector<SWD_ParticlePosInfo>& particles,
|
||
int* errCode)
|
||
{
|
||
int lineNum = (int)scanLines.size();
|
||
if (lineNum == 0)
|
||
{
|
||
*errCode = SG_ERR_3D_DATA_NULL;
|
||
return;
|
||
}
|
||
//噪点过滤
|
||
//垂直方向过滤
|
||
wd_noiseFilter(scanLines, algoParam, errCode);
|
||
if (*errCode != 0)
|
||
return;
|
||
|
||
///开始数据处理
|
||
//提取特征：跳变、低于z阈值、V及L型
|
||
//垂直数据处理
|
||
std::vector<SSG_lineFeature> all_vLineFeatures;
|
||
for (int i = 0; i < lineNum; i++)
|
||
{
|
||
if (i == 202)
|
||
int k = 1;
|
||
SSG_lineFeature a_line_features;
|
||
a_line_features.lineIdx = i;
|
||
wd_getLineCornerFeature_PSM(
|
||
&scanLines[i][0],
|
||
(int)scanLines[i].size(),
|
||
i,
|
||
groundCalibPara.planeHeight,
|
||
algoParam.cornerParam,
|
||
&a_line_features);
|
||
|
||
all_vLineFeatures.push_back(a_line_features); //空行也加入，保证能按行号索引
|
||
}
|
||
//生成水平扫描数据
|
||
int nPointCnt = (int)scanLines[0].size();
|
||
std::vector<std::vector<SVzNL3DPosition>> hLines;
|
||
hLines.resize(nPointCnt);
|
||
for (int i = 0; i < nPointCnt; i++)
|
||
hLines[i].resize(lineNum);
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
for (int j = 0; j < nPointCnt; j++)
|
||
{
|
||
scanLines[line][j].nPointIdx = 0;;
|
||
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;
|
||
}
|
||
}
|
||
std::vector<SSG_lineFeature> all_hLineFeatures;
|
||
//逐行提取特征
|
||
for (int hLine = 0; hLine < nPointCnt; hLine++)
|
||
{
|
||
if (hLine == 14)
|
||
int kkk = 1;
|
||
|
||
SSG_lineFeature a_hLine_featrues;
|
||
a_hLine_featrues.lineIdx = hLine;
|
||
|
||
wd_getLineCornerFeature_PSM(
|
||
&hLines[hLine][0],
|
||
(int)hLines[hLine].size(),
|
||
hLine,
|
||
groundCalibPara.planeHeight,
|
||
algoParam.cornerParam,
|
||
&a_hLine_featrues);
|
||
|
||
//if ((a_hLine_featrues.features.size() > 0) || (a_hLine_featrues.endings.size() > 0))
|
||
all_hLineFeatures.push_back(a_hLine_featrues);//空行也加入，保证能按行号索引
|
||
}
|
||
|
||
/// 统计整个视野大小
|
||
SWD_pointCloudPara pntCloudPara = wd_getPointCloudPara(scanLines);
|
||
SVzNLRangeD x_range = pntCloudPara.xRange;
|
||
SVzNLRangeD y_range = pntCloudPara.yRange;
|
||
|
||
SSG_ROIRectD globalROI;
|
||
globalROI.left = pntCloudPara.xRange.min;
|
||
globalROI.right = pntCloudPara.xRange.max;
|
||
globalROI.top = pntCloudPara.yRange.min;
|
||
globalROI.bottom = pntCloudPara.yRange.max;
|
||
|
||
//垂直方向特征生长（相机扫描方向）
|
||
std::vector<SSG_featureTree> v_feature_trees;
|
||
std::vector<SSG_featureTree> v_ending_trees;
|
||
//特征生长
|
||
wd_getFeatureGrowingTrees_noTypeMatch(
|
||
all_vLineFeatures,
|
||
v_feature_trees,
|
||
v_ending_trees,
|
||
algoParam.growParam);
|
||
|
||
//水平方向特征生长
|
||
std::vector<SSG_featureTree> h_feature_trees;
|
||
std::vector<SSG_featureTree> h_ending_trees;
|
||
//特征生长
|
||
wd_getFeatureGrowingTrees_noTypeMatch(
|
||
all_hLineFeatures,
|
||
h_feature_trees,
|
||
h_ending_trees,
|
||
algoParam.growParam);
|
||
|
||
//生成Mask
|
||
double scale;
|
||
if ((pntCloudPara.scale_x < 0) || (pntCloudPara.scale_y < 0))
|
||
{
|
||
*errCode = SG_ERR_3D_DATA_INVLD;
|
||
return;
|
||
}
|
||
if(pntCloudPara.scale_x < pntCloudPara.scale_y)
|
||
scale = pntCloudPara.scale_x;
|
||
else
|
||
scale = pntCloudPara.scale_y;
|
||
|
||
|
||
//距离变换Mask，以1mm为量化尺度
|
||
double inerPolateDistTh = scale * 10; //插值门限， 两间距离大于此阈值不插值
|
||
int edgeSkip = 2;
|
||
int maskX = (int)(x_range.max - x_range.min)/scale + 1;
|
||
int maskY = (int)(y_range.max - y_range.min)/scale + 1;
|
||
if ((maskX < 16) || (maskY < 16))
|
||
return;
|
||
maskY = maskY + edgeSkip * 2;
|
||
maskX = maskX + edgeSkip * 2;
|
||
cv::Mat distTranformMask(maskY, maskX, CV_32FC1, 0.0f); //距离变换Mask，初始化为一个极大值1e+6
|
||
cv::Mat distTranformIndexing(maskY, maskX, CV_32SC2, cv::Vec2i(0, 0)); //坐标索引
|
||
cv::Mat featureMask = cv::Mat::zeros(nPointCnt, lineNum, CV_32SC4);
|
||
pointClout2DProjection(
|
||
scanLines,
|
||
x_range,
|
||
y_range,
|
||
scale,
|
||
edgeSkip,
|
||
inerPolateDistTh, //插值阈值。大于此阈值的不进行量化插值
|
||
distTranformMask,//投影量化数据，初始化为一个极大值1e+6
|
||
distTranformIndexing //标记坐标索引，用于回找3D坐标
|
||
);
|
||
|
||
std::vector<SSG_treeInfo> allTreesInfo; //不包含边界
|
||
//标注:垂直
|
||
SSG_treeInfo a_nullTree;
|
||
memset(&a_nullTree, 0, sizeof(SSG_treeInfo));
|
||
allTreesInfo.push_back(a_nullTree); //保持存储位置与treeIdx相同位置，方便索引
|
||
//标注垂直边界
|
||
int treeID = 1;
|
||
for (int i = 0, i_max = (int)v_ending_trees.size(); i < i_max; i++)
|
||
{
|
||
SSG_featureTree* a_vEdgeTree = &v_ending_trees[i];
|
||
|
||
//记录Tree的信息
|
||
SSG_treeInfo a_treeInfo;
|
||
a_treeInfo.vTreeFlag = 1;
|
||
a_treeInfo.treeIdx = treeID;
|
||
a_treeInfo.treeType = a_vEdgeTree->treeType;
|
||
a_treeInfo.sLineIdx = a_vEdgeTree->sLineIdx;
|
||
a_treeInfo.eLineIdx = a_vEdgeTree->eLineIdx;
|
||
a_treeInfo.roi = a_vEdgeTree->roi;
|
||
allTreesInfo.push_back(a_treeInfo);
|
||
//在原始点云上标记，同时有Mask上标记
|
||
for (int j = 0, j_max = (int)a_vEdgeTree->treeNodes.size(); j < j_max; j++)
|
||
{
|
||
SSG_basicFeature1D* a_feature = &a_vEdgeTree->treeNodes[j];
|
||
if (scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].pt3D.z > 1e-4)//虚假目标过滤后点会置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 += treeID << 16;
|
||
featureMask.at<cv::Vec4i>(a_feature->jumpPos2D.y, a_feature->jumpPos2D.x)[0] = treeID; //edgeID
|
||
featureMask.at<cv::Vec4i>(a_feature->jumpPos2D.y, a_feature->jumpPos2D.x)[1] = a_vEdgeTree->treeType;
|
||
featureMask.at<cv::Vec4i>(a_feature->jumpPos2D.y, a_feature->jumpPos2D.x)[2] = 1; //vscan
|
||
|
||
int px = (int)((scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].pt3D.x - x_range.min)/scale) + edgeSkip;
|
||
int py = (int)((scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].pt3D.y - y_range.min)/scale) + edgeSkip;
|
||
distTranformMask.at<float>(py, px) = 0;
|
||
}
|
||
}
|
||
treeID++;
|
||
}
|
||
//标注水平边界
|
||
for (int i = 0, i_max = (int)h_ending_trees.size(); i < i_max; i++)
|
||
{
|
||
SSG_featureTree* a_hEdgeTree = &h_ending_trees[i];
|
||
//记录Tree的信息
|
||
SSG_treeInfo a_treeInfo;
|
||
a_treeInfo.vTreeFlag = 0;
|
||
a_treeInfo.treeIdx = treeID;
|
||
a_treeInfo.treeType = a_hEdgeTree->treeType;
|
||
a_treeInfo.sLineIdx = a_hEdgeTree->sLineIdx;
|
||
a_treeInfo.eLineIdx = a_hEdgeTree->eLineIdx;
|
||
a_treeInfo.roi.left = a_hEdgeTree->roi.top; //水平扫描xy是交换的
|
||
a_treeInfo.roi.right = a_hEdgeTree->roi.bottom;
|
||
a_treeInfo.roi.top = a_hEdgeTree->roi.left;
|
||
a_treeInfo.roi.bottom = a_hEdgeTree->roi.right;
|
||
allTreesInfo.push_back(a_treeInfo);
|
||
//在原始点云上标记，同时有Mask上标记
|
||
for (int j = 0, j_max = (int)a_hEdgeTree->treeNodes.size(); j < j_max; j++)
|
||
{
|
||
SSG_basicFeature1D* a_feature = &a_hEdgeTree->treeNodes[j];
|
||
if (scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].pt3D.z > 1e-4)//虚假目标过滤后点会置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 += treeID << 16;
|
||
featureMask.at<cv::Vec4i>(a_feature->jumpPos2D.x, a_feature->jumpPos2D.y)[0] = treeID;
|
||
featureMask.at<cv::Vec4i>(a_feature->jumpPos2D.x, a_feature->jumpPos2D.y)[1] += a_hEdgeTree->treeType << 4;
|
||
featureMask.at<cv::Vec4i>(a_feature->jumpPos2D.x, a_feature->jumpPos2D.y)[2] = 2;//hsan flag
|
||
|
||
int px = (int)((scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].pt3D.x - x_range.min)/scale) + edgeSkip;
|
||
int py = (int)((scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].pt3D.y - y_range.min)/scale) + edgeSkip;
|
||
distTranformMask.at<float>(py, px) = 0;
|
||
}
|
||
}
|
||
}
|
||
treeID++;
|
||
}
|
||
//垂直特征标注
|
||
int hvTreeIdx = treeID;
|
||
int vTreeStart = treeID;
|
||
for (int i = 0, i_max = (int)v_feature_trees.size(); i < i_max; i++)
|
||
{
|
||
SSG_featureTree* a_vTree = &v_feature_trees[i];
|
||
|
||
//记录Tree的信息
|
||
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);
|
||
//在原始点云上标记，同时有Mask上标记
|
||
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)//虚假目标过滤后点会置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;
|
||
featureMask.at<cv::Vec4i>(a_feature->jumpPos2D.y, a_feature->jumpPos2D.x)[0] = hvTreeIdx; //edgeID
|
||
featureMask.at<cv::Vec4i>(a_feature->jumpPos2D.y, a_feature->jumpPos2D.x)[1] = a_vTree->treeType;
|
||
featureMask.at<cv::Vec4i>(a_feature->jumpPos2D.y, a_feature->jumpPos2D.x)[2] = 1; //vscan
|
||
|
||
int px = (int)((scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].pt3D.x - x_range.min)/scale) + edgeSkip;
|
||
int py = (int)((scanLines[a_feature->jumpPos2D.x][a_feature->jumpPos2D.y].pt3D.y - y_range.min)/scale) + edgeSkip;
|
||
distTranformMask.at<float>(py, px) = 0;
|
||
}
|
||
}
|
||
}
|
||
hvTreeIdx++;
|
||
}
|
||
int hTreeStart = hvTreeIdx;
|
||
//标注:水平特征
|
||
for (int i = 0, i_max = (int)h_feature_trees.size(); i < i_max; i++)
|
||
{
|
||
SSG_featureTree* a_hTree = &h_feature_trees[i];
|
||
//记录Tree的信息
|
||
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; //水平扫描xy是交换的
|
||
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);
|
||
//在原始点云上标记，同时有Mask上标记
|
||
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)//虚假目标过滤后点会置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;
|
||
featureMask.at<cv::Vec4i>(a_feature->jumpPos2D.x, a_feature->jumpPos2D.y)[0] = hvTreeIdx;
|
||
featureMask.at<cv::Vec4i>(a_feature->jumpPos2D.x, a_feature->jumpPos2D.y)[1] += a_hTree->treeType << 4;
|
||
featureMask.at<cv::Vec4i>(a_feature->jumpPos2D.x, a_feature->jumpPos2D.y)[2] = 2;//hsan flag
|
||
|
||
int px = (int)((scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].pt3D.x - x_range.min)/scale) + edgeSkip;
|
||
int py = (int)((scanLines[a_feature->jumpPos2D.y][a_feature->jumpPos2D.x].pt3D.y - y_range.min)/scale) + edgeSkip;
|
||
distTranformMask.at<float>(py, px) = 0;
|
||
}
|
||
}
|
||
}
|
||
hvTreeIdx++;
|
||
}
|
||
int hvTreeSize = hvTreeIdx;
|
||
double x_scale = pntCloudPara.scale_x;
|
||
double y_scale = pntCloudPara.scale_y;
|
||
|
||
//进行距离变换，然后使用分水岭算法进行分割
|
||
cv::Mat distTransform;
|
||
sg_distanceTrans(distTranformMask, distTransform, 0);
|
||
#if OUTPUT_DEBUG //debug
|
||
cv::Mat maskImage;
|
||
cv::normalize(distTranformMask, maskImage, 0, 255, cv::NORM_MINMAX, CV_8U);
|
||
cv::imwrite("distTransformMask.png", maskImage);
|
||
cv::Mat dtImage;
|
||
cv::normalize(distTransform, dtImage, 0, 255, cv::NORM_MINMAX, CV_8U);
|
||
cv::imwrite("distTransform.png", dtImage);
|
||
#endif
|
||
|
||
//寻找Peak。Peak确定后以Peak为种子点进行分水岭方法分割
|
||
double minW = particleSizeParam.minSize.width;
|
||
SSG_localPkParam searchWin;
|
||
searchWin.seachW_lines = (int)(minW * 0.4/scale);
|
||
searchWin.searchW_pts = (int)(minW * 0.4/scale);
|
||
std::vector<SSG_2DValueI> dt_peaks;
|
||
sg_getLocalPeaks_distTransform(distTransform, dt_peaks, searchWin);
|
||
//以大小进行过滤
|
||
double minDistTh = minW * 0.4 / scale;
|
||
std::vector<SSG_2DValueI> filter_dt_peaks;
|
||
for (int i = 0, i_max = (int)dt_peaks.size(); i < i_max; i++)
|
||
{
|
||
if (dt_peaks[i].valueD > minDistTh)
|
||
filter_dt_peaks.push_back(dt_peaks[i]);
|
||
}
|
||
//以距离进行过滤：距离变换相当于最小内切圆。不同的目标所在的内切圆一定不相交，因此：R1+R2 < 圆心距离
|
||
int filterSize = (int)filter_dt_peaks.size();
|
||
for (int i = 0; i < filterSize; i++)
|
||
{
|
||
for (int j = i + 1; j < filterSize; j++)
|
||
{
|
||
|
||
}
|
||
}
|
||
|
||
//分水岭分割
|
||
|
||
//扫描边界，建立相邻目标边界集合
|
||
//通过目标相邻边界判断是否合并相邻目标
|
||
|
||
|
||
//获取最大最小值
|
||
double minVal, maxVal;
|
||
cv::Point minLoc, maxLoc;
|
||
// 调用minMaxLoc函数
|
||
cv::minMaxLoc(distTransform, &minVal, &maxVal, &minLoc, &maxLoc);
|
||
|
||
//分水岭算法进行分割
|
||
SWD_waterShedImage wsImg;
|
||
wsImg.width = distTransform.cols;
|
||
wsImg.height = distTransform.rows;
|
||
wsImg.gray.resize(wsImg.height, std::vector<int>(wsImg.width));
|
||
wsImg.markers.resize(wsImg.height, std::vector<int>(wsImg.width, 1)); // 初始化标记图为0
|
||
int maxValue = (int)maxVal + 2;
|
||
for (int i = 0; i < distTransform.rows; i++)
|
||
{
|
||
float* rowPtr = distTransform.ptr<float>(i);
|
||
for (int j = 0; j < distTransform.cols; j++)
|
||
{
|
||
float disValue = rowPtr[j];
|
||
if (disValue < 1e-4) //边界和背景
|
||
wsImg.gray[i][j] = maxValue;
|
||
else
|
||
{
|
||
wsImg.gray[i][j] = (int)(maxVal - disValue);
|
||
wsImg.markers[i][j] = 0;
|
||
}
|
||
}
|
||
}
|
||
|
||
watershed(wsImg);
|
||
|
||
#if OUTPUT_DEBUG //debug
|
||
cv::Mat waterShedResult(wsImg.height, wsImg.width, CV_8UC3);
|
||
for (int i = 0; i < wsImg.height; ++i) {
|
||
for (int j = 0; j < wsImg.width; ++j) {
|
||
if (wsImg.markers[i][j] == -1) { // 分水岭边界（红色）
|
||
waterShedResult.at<cv::Vec3b>(i, j) = cv::Vec3b(0,0,255);
|
||
}
|
||
else { // 区域（根据标记值生成不同颜色）
|
||
int color_r = (wsImg.markers[i][j] * 50) % 256;
|
||
int color_g = (color_r + 85) % 256;
|
||
int color_b = (color_r + 170) % 256;
|
||
waterShedResult.at<cv::Vec3b>(i, j) = cv::Vec3b(color_b, color_g, color_r);
|
||
}
|
||
}
|
||
}
|
||
cv::imwrite("watershed.png", waterShedResult);
|
||
#endif
|
||
#if 0
|
||
|
||
|
||
|
||
|
||
|
||
|
||
SSG_localPkParam searchWin;
|
||
searchWin.seachW_lines = (int)(algoParam.bagParam.bagW * 0.4);
|
||
searchWin.searchW_pts = (int)(algoParam.bagParam.bagW * 0.4);
|
||
std::vector<SSG_2DValueI> dt_peaks;
|
||
sg_getLocalPeaks_distTransform(distTransform, dt_peaks, searchWin);
|
||
//获取Peaks
|
||
int invlidDistToEdge = 50; //距离边缘近的Peak是非法点。
|
||
double minPeakValue = algoParam.bagParam.bagW / 8;
|
||
std::vector<SSG_2DValueI> peaks;
|
||
for (int i = 0; i < dt_peaks.size(); i++)
|
||
{
|
||
//边界处的Peak为不合格Peak，去除
|
||
int x_diff_0 = dt_peaks[i].x; //距左边距离，单位为mm（量化尺度为1mm）
|
||
int x_diff_1 = distTransform.cols - dt_peaks[i].x;//距右边距离
|
||
int y_diff_0 = dt_peaks[i].y;//距上边距离
|
||
int y_diff_1 = distTransform.rows - dt_peaks[i].y;//距下边距离
|
||
if ((x_diff_0 < invlidDistToEdge) || (x_diff_1 < invlidDistToEdge) ||
|
||
(y_diff_0 < invlidDistToEdge) || (y_diff_1 < invlidDistToEdge) ||
|
||
(dt_peaks[i].valueD < minPeakValue))
|
||
continue;
|
||
|
||
//在distTranformIndexing中回找坐标
|
||
double pkValue = dt_peaks[i].valueD;
|
||
SSG_2DValueI a_peak = _backIndexingPeakPos(dt_peaks[i], distTranformIndexing);
|
||
a_peak.valueD = pkValue; // laser3DPoints[peaks[i].x].p3DPosition[peaks[i].y].pt3D.z;
|
||
peaks.push_back(a_peak);
|
||
}
|
||
//按照高度排序
|
||
std::sort(peaks.begin(), peaks.end(), compareByHeight);
|
||
for (int i = 0, i_max = (int)peaks.size(); i < i_max; i++)
|
||
featureMask.at<cv::Vec4i>(peaks[i].y, peaks[i].x)[3] = 1; //peak flag
|
||
|
||
#if 0
|
||
//使用真实的z值代替距离变换值
|
||
for (int i = 0, i_max = peaks.size(); i < i_max; i++)
|
||
{
|
||
peaks[i].valueD = laser3DPoints[peaks[i].x].p3DPosition[peaks[i].y].pt3D.z;
|
||
}
|
||
#endif
|
||
|
||
/// 以区域最高点作为种子进行区域生长
|
||
std::vector<SSG_peakRgnInfo> peakRgns;
|
||
int peakRgnId = 1;
|
||
for (int i = 0, i_max = (int)peaks.size(); i < i_max; i++)
|
||
{
|
||
if (i == 3)
|
||
int kkk = 1;
|
||
|
||
SVzNL3DPosition* pk_pt = &(laser3DPoints[peaks[i].x].p3DPosition[peaks[i].y]);
|
||
int pkRgnId = (pk_pt->nPointIdx >> 8) & 0xff;
|
||
if (pkRgnId > 0)
|
||
continue;
|
||
|
||
//生长
|
||
//进行水平和垂直方向扫描得到边界点
|
||
std::vector< SSG_lineConotours> topContour;
|
||
std::vector< SSG_lineConotours> bottomContour;
|
||
std::vector< SSG_lineConotours> leftContour;
|
||
std::vector< SSG_lineConotours> rightContour;
|
||
int maxEdgeId_top = 0, maxEdgeId_btm = 0, maxEdgeId_left = 0, maxEdgeId_right = 0;
|
||
sg_peakXYScan(
|
||
laser3DPoints,
|
||
lineNum,
|
||
featureMask,
|
||
peaks[i],
|
||
algoParam.growParam,
|
||
algoParam.bagParam,
|
||
false,
|
||
topContour,
|
||
bottomContour,
|
||
leftContour,
|
||
rightContour,
|
||
&maxEdgeId_top,
|
||
&maxEdgeId_btm,
|
||
&maxEdgeId_left,
|
||
&maxEdgeId_right);
|
||
|
||
|
||
int vldSide = 0;
|
||
if (leftContour.size() > 30)
|
||
vldSide++;
|
||
if (rightContour.size() > 30)
|
||
vldSide++;
|
||
if (topContour.size() > 30)
|
||
vldSide++;
|
||
if (bottomContour.size() > 30)
|
||
vldSide++;
|
||
|
||
int invldSide = 0;
|
||
if (leftContour.size() < 10)
|
||
invldSide++;
|
||
if (rightContour.size() < 10)
|
||
invldSide++;
|
||
if (topContour.size() < 10)
|
||
invldSide++;
|
||
if (bottomContour.size() < 10)
|
||
invldSide++;
|
||
|
||
if ((vldSide < 3) || (invldSide > 0))
|
||
continue;
|
||
|
||
//全匹配：对于褶皱较多的袋子，需要全匹配寻找到正确的边
|
||
std::vector<SSG_edgeMatchInfo> matchTable_TB;
|
||
std::vector< SSG_matchPair> TB_pairs;
|
||
std::vector<SSG_conotourPair> TB_contourPairs;
|
||
int TB_matchNum = 0;
|
||
_getMatchTable(
|
||
topContour,
|
||
bottomContour,
|
||
maxEdgeId_top,
|
||
maxEdgeId_btm,
|
||
true, //isVScan,
|
||
vTreeStart,
|
||
hTreeStart,
|
||
matchTable_TB,
|
||
TB_pairs,
|
||
TB_contourPairs,
|
||
&TB_matchNum);
|
||
if (TB_matchNum < 25)
|
||
continue;
|
||
|
||
std::vector< SSG_matchPair> LR_pairs;
|
||
std::vector<SSG_edgeMatchInfo> matchTable_LR;
|
||
std::vector<SSG_conotourPair> LR_contourPairs;
|
||
int LR_matchNum = 0;
|
||
_getMatchTable(
|
||
leftContour,
|
||
rightContour,
|
||
maxEdgeId_left,
|
||
maxEdgeId_right,
|
||
false, //isHScan,
|
||
vTreeStart,
|
||
hTreeStart,
|
||
matchTable_LR,
|
||
LR_pairs,
|
||
LR_contourPairs,
|
||
&LR_matchNum);
|
||
|
||
if (LR_matchNum < 25)
|
||
continue;
|
||
|
||
int lowLevelChkFlag = 0;
|
||
SSG_peakRgnInfo a_pkRgn = _maxLikelihoodMatch(
|
||
laser3DPoints,
|
||
lineNum,
|
||
hvTreeSize,
|
||
peaks[i],
|
||
matchTable_TB,
|
||
TB_pairs,
|
||
TB_contourPairs,
|
||
TB_matchNum,
|
||
maxEdgeId_btm,
|
||
matchTable_LR,
|
||
LR_pairs,
|
||
LR_contourPairs,
|
||
LR_matchNum,
|
||
maxEdgeId_right,
|
||
allTreesInfo,
|
||
vTreeStart,
|
||
hTreeStart,
|
||
globalROI,
|
||
algoParam,
|
||
peakRgnId,
|
||
&lowLevelChkFlag);
|
||
if (a_pkRgn.pkRgnIdx > 0)
|
||
{
|
||
peakRgns.push_back(a_pkRgn);
|
||
peakRgnId++;
|
||
}
|
||
}
|
||
#if 1
|
||
///迭代处理！！！！保证没有大于袋子的目标未处理
|
||
///对于剩下的目标，如果没有检出的使用推理方法进行。
|
||
///扫描时检测保证水平或垂直方向的宽度是否满足袋子尺寸，然后在另一个方向进行推理方法检测
|
||
while (1)
|
||
{
|
||
std::vector<SSG_peakRgnInfo> iter_objs;
|
||
//将没有处理的Peak点保留
|
||
std::vector<SSG_2DValueI> residualPeaks;
|
||
for (int i = 0, i_max = (int)peaks.size(); i < i_max; i++)
|
||
{
|
||
SVzNL3DPosition* pk_pt = &(laser3DPoints[peaks[i].x].p3DPosition[peaks[i].y]);
|
||
int pkRgnId = (pk_pt->nPointIdx >> 8) & 0xff;
|
||
if (pkRgnId == 0)
|
||
{
|
||
residualPeaks.push_back(peaks[i]);
|
||
}
|
||
}
|
||
if (residualPeaks.size() == 0)
|
||
break;
|
||
|
||
bool rgnPtAsEdge = true;
|
||
for (int ri = 0; ri < residualPeaks.size(); ri++)
|
||
{
|
||
SVzNL3DPosition* pk_pt = &(laser3DPoints[residualPeaks[ri].x].p3DPosition[residualPeaks[ri].y]);
|
||
int pkRgnId = (pk_pt->nPointIdx >> 8) & 0xff;
|
||
if (pkRgnId > 0)
|
||
continue;
|
||
//生长
|
||
//进行水平和垂直方向扫描得到边界点
|
||
std::vector< SSG_lineConotours> resi_topContour;
|
||
std::vector< SSG_lineConotours> resi_bottomContour;
|
||
std::vector< SSG_lineConotours> resi_leftContour;
|
||
std::vector< SSG_lineConotours> resi_rightContour;
|
||
int resi_maxEdgeId_top = 0, resi_maxEdgeId_btm = 0, resi_maxEdgeId_left = 0, resi_maxEdgeId_right = 0;
|
||
sg_peakXYScan(
|
||
laser3DPoints,
|
||
lineNum,
|
||
featureMask,
|
||
residualPeaks[ri],
|
||
algoParam.growParam,
|
||
algoParam.bagParam,
|
||
true,
|
||
resi_topContour,
|
||
resi_bottomContour,
|
||
resi_leftContour,
|
||
resi_rightContour,
|
||
&resi_maxEdgeId_top,
|
||
&resi_maxEdgeId_btm,
|
||
&resi_maxEdgeId_left,
|
||
&resi_maxEdgeId_right);
|
||
|
||
if ((resi_topContour.size() == 0) || (resi_bottomContour.size() == 0) || (resi_leftContour.size() == 0) || (resi_rightContour.size() == 0))
|
||
continue;
|
||
|
||
//分段计算平均宽度和平均高度以及分段ROI
|
||
//全匹配（将水平所有分段的所有可能距离
|
||
std::vector<SSG_edgeMatchInfo> matchTable_TB;
|
||
std::vector< SSG_matchPair> TB_pairs;
|
||
std::vector<SSG_conotourPair> TB_contourPairs;
|
||
int TB_matchNum = 0;
|
||
_getMatchTable(
|
||
resi_topContour,
|
||
resi_bottomContour,
|
||
resi_maxEdgeId_top,
|
||
resi_maxEdgeId_btm,
|
||
true, //isVScan,
|
||
vTreeStart,
|
||
hTreeStart,
|
||
matchTable_TB,
|
||
TB_pairs,
|
||
TB_contourPairs,
|
||
&TB_matchNum);
|
||
|
||
if (TB_matchNum < 25)
|
||
continue;
|
||
|
||
std::vector< SSG_matchPair> LR_pairs;
|
||
std::vector<SSG_edgeMatchInfo> matchTable_LR;
|
||
std::vector<SSG_conotourPair> LR_contourPairs;
|
||
int LR_matchNum = 0;
|
||
_getMatchTable(
|
||
resi_leftContour,
|
||
resi_rightContour,
|
||
resi_maxEdgeId_left,
|
||
resi_maxEdgeId_right,
|
||
false, //isHScan,
|
||
vTreeStart,
|
||
hTreeStart,
|
||
matchTable_LR,
|
||
LR_pairs,
|
||
LR_contourPairs,
|
||
&LR_matchNum);
|
||
|
||
if (LR_matchNum < 25)
|
||
continue;
|
||
|
||
int lowLevelChkFlag = 0;
|
||
SSG_peakRgnInfo a_pkRgn = _maxLikelihoodMatch(
|
||
laser3DPoints,
|
||
lineNum,
|
||
hvTreeSize,
|
||
peaks[ri],
|
||
matchTable_TB,
|
||
TB_pairs,
|
||
TB_contourPairs,
|
||
TB_matchNum,
|
||
resi_maxEdgeId_btm,
|
||
matchTable_LR,
|
||
LR_pairs,
|
||
LR_contourPairs,
|
||
LR_matchNum,
|
||
resi_maxEdgeId_right,
|
||
allTreesInfo,
|
||
vTreeStart,
|
||
hTreeStart,
|
||
globalROI,
|
||
algoParam,
|
||
peakRgnId,
|
||
&lowLevelChkFlag);
|
||
#if 0
|
||
if (lowLevelChkFlag > 0)
|
||
{
|
||
//lowLevelFlag_T + lowLevelFlag_B<<1 + lowLevelFlag_L<<2 + lowLevelFlag_R<<3;
|
||
if (lowLevelChkFlag & 0x01) //Top
|
||
{
|
||
|
||
}
|
||
if (lowLevelChkFlag & 0x02) //Bottom
|
||
{
|
||
}
|
||
if (lowLevelChkFlag & 0x04) //Left
|
||
{
|
||
}
|
||
if (lowLevelChkFlag & 0x08) //Rigjt
|
||
{
|
||
}
|
||
}
|
||
#endif
|
||
if (a_pkRgn.pkRgnIdx > 0)
|
||
{
|
||
iter_objs.push_back(a_pkRgn);
|
||
peakRgnId++;
|
||
}
|
||
}
|
||
if (iter_objs.size() == 0)
|
||
break;
|
||
|
||
peakRgns.insert(peakRgns.end(), iter_objs.begin(), iter_objs.end());
|
||
//为下一次迭代准备
|
||
peaks.clear();
|
||
peaks.insert(peaks.end(), residualPeaks.begin(), residualPeaks.end());
|
||
}
|
||
|
||
///将剩余的小目标检出，用于碰撞检测
|
||
///不能有未知的未处理的区域，以防止未知的碰撞
|
||
///记录所有的大于1/4L*1/4W的目标
|
||
std::vector<SSG_2DValueI> smallObjPeaks; //记录0.25L * 0.25W的目标，用于碰撞检查
|
||
//将最后没有处理的Peak点保留
|
||
std::vector<SSG_2DValueI> residualPeaks;
|
||
for (int i = 0, i_max = (int)peaks.size(); i < i_max; i++)
|
||
{
|
||
SVzNL3DPosition* pk_pt = &(laser3DPoints[peaks[i].x].p3DPosition[peaks[i].y]);
|
||
int pkRgnId = (pk_pt->nPointIdx >> 8) & 0xff;
|
||
if (pkRgnId == 0)
|
||
{
|
||
residualPeaks.push_back(peaks[i]);
|
||
}
|
||
}
|
||
if (residualPeaks.size() > 0)
|
||
{
|
||
bool rgnPtAsEdge = true;
|
||
for (int ri = 0; ri < residualPeaks.size(); ri++)
|
||
{
|
||
//生长
|
||
//进行水平和垂直方向扫描得到边界点
|
||
std::vector< SSG_lineConotours> resi_topContour;
|
||
std::vector< SSG_lineConotours> resi_bottomContour;
|
||
std::vector< SSG_lineConotours> resi_leftContour;
|
||
std::vector< SSG_lineConotours> resi_rightContour;
|
||
int resi_maxEdgeId_top = 0, resi_maxEdgeId_btm = 0, resi_maxEdgeId_left = 0, resi_maxEdgeId_right = 0;
|
||
sg_peakXYScan(
|
||
laser3DPoints,
|
||
lineNum,
|
||
featureMask,
|
||
residualPeaks[ri],
|
||
algoParam.growParam,
|
||
algoParam.bagParam,
|
||
true,
|
||
resi_topContour,
|
||
resi_bottomContour,
|
||
resi_leftContour,
|
||
resi_rightContour,
|
||
&resi_maxEdgeId_top,
|
||
&resi_maxEdgeId_btm,
|
||
&resi_maxEdgeId_left,
|
||
&resi_maxEdgeId_right);
|
||
//计算ROI
|
||
//保留长宽都大于门限的
|
||
SSG_ROIRectD objROI = { 0, -1, 0, 0 };
|
||
for (int n = 0; n < resi_topContour.size(); n++)
|
||
{
|
||
std::vector<SSG_contourPtInfo>& a_line_contourPts = resi_topContour[n].contourPts;
|
||
for (int m = 0; m < a_line_contourPts.size(); m++)
|
||
{
|
||
if (objROI.right < objROI.left)
|
||
{
|
||
objROI.left = a_line_contourPts[m].edgePt.x;
|
||
objROI.right = a_line_contourPts[m].edgePt.x;
|
||
objROI.top = a_line_contourPts[m].edgePt.y;
|
||
objROI.bottom = a_line_contourPts[m].edgePt.y;
|
||
}
|
||
else
|
||
{
|
||
objROI.left = objROI.left > a_line_contourPts[m].edgePt.x ? a_line_contourPts[m].edgePt.x : objROI.left;
|
||
objROI.right = objROI.right < a_line_contourPts[m].edgePt.x ? a_line_contourPts[m].edgePt.x : objROI.right;
|
||
objROI.top = objROI.top > a_line_contourPts[m].edgePt.y ? a_line_contourPts[m].edgePt.y : objROI.top;
|
||
objROI.bottom = objROI.bottom < a_line_contourPts[m].edgePt.y ? a_line_contourPts[m].edgePt.y : objROI.bottom;
|
||
}
|
||
}
|
||
}
|
||
for (int n = 0; n < resi_bottomContour.size(); n++)
|
||
{
|
||
std::vector<SSG_contourPtInfo>& a_line_contourPts = resi_bottomContour[n].contourPts;
|
||
for (int m = 0; m < a_line_contourPts.size(); m++)
|
||
{
|
||
if (objROI.right < objROI.left)
|
||
{
|
||
objROI.left = a_line_contourPts[m].edgePt.x;
|
||
objROI.right = a_line_contourPts[m].edgePt.x;
|
||
objROI.top = a_line_contourPts[m].edgePt.y;
|
||
objROI.bottom = a_line_contourPts[m].edgePt.y;
|
||
}
|
||
else
|
||
{
|
||
objROI.left = objROI.left > a_line_contourPts[m].edgePt.x ? a_line_contourPts[m].edgePt.x : objROI.left;
|
||
objROI.right = objROI.right < a_line_contourPts[m].edgePt.x ? a_line_contourPts[m].edgePt.x : objROI.right;
|
||
objROI.top = objROI.top > a_line_contourPts[m].edgePt.y ? a_line_contourPts[m].edgePt.y : objROI.top;
|
||
objROI.bottom = objROI.bottom < a_line_contourPts[m].edgePt.y ? a_line_contourPts[m].edgePt.y : objROI.bottom;
|
||
}
|
||
}
|
||
}
|
||
for (int n = 0; n < resi_leftContour.size(); n++)
|
||
{
|
||
std::vector<SSG_contourPtInfo>& a_line_contourPts = resi_leftContour[n].contourPts;
|
||
for (int m = 0; m < a_line_contourPts.size(); m++)
|
||
{
|
||
if (objROI.right < objROI.left)
|
||
{
|
||
objROI.left = a_line_contourPts[m].edgePt.x;
|
||
objROI.right = a_line_contourPts[m].edgePt.x;
|
||
objROI.top = a_line_contourPts[m].edgePt.y;
|
||
objROI.bottom = a_line_contourPts[m].edgePt.y;
|
||
}
|
||
else
|
||
{
|
||
objROI.left = objROI.left > a_line_contourPts[m].edgePt.x ? a_line_contourPts[m].edgePt.x : objROI.left;
|
||
objROI.right = objROI.right < a_line_contourPts[m].edgePt.x ? a_line_contourPts[m].edgePt.x : objROI.right;
|
||
objROI.top = objROI.top > a_line_contourPts[m].edgePt.y ? a_line_contourPts[m].edgePt.y : objROI.top;
|
||
objROI.bottom = objROI.bottom < a_line_contourPts[m].edgePt.y ? a_line_contourPts[m].edgePt.y : objROI.bottom;
|
||
}
|
||
}
|
||
}
|
||
for (int n = 0; n < resi_rightContour.size(); n++)
|
||
{
|
||
std::vector<SSG_contourPtInfo>& a_line_contourPts = resi_rightContour[n].contourPts;
|
||
for (int m = 0; m < a_line_contourPts.size(); m++)
|
||
{
|
||
if (objROI.right < objROI.left)
|
||
{
|
||
objROI.left = a_line_contourPts[m].edgePt.x;
|
||
objROI.right = a_line_contourPts[m].edgePt.x;
|
||
objROI.top = a_line_contourPts[m].edgePt.y;
|
||
objROI.bottom = a_line_contourPts[m].edgePt.y;
|
||
}
|
||
else
|
||
{
|
||
objROI.left = objROI.left > a_line_contourPts[m].edgePt.x ? a_line_contourPts[m].edgePt.x : objROI.left;
|
||
objROI.right = objROI.right < a_line_contourPts[m].edgePt.x ? a_line_contourPts[m].edgePt.x : objROI.right;
|
||
objROI.top = objROI.top > a_line_contourPts[m].edgePt.y ? a_line_contourPts[m].edgePt.y : objROI.top;
|
||
objROI.bottom = objROI.bottom < a_line_contourPts[m].edgePt.y ? a_line_contourPts[m].edgePt.y : objROI.bottom;
|
||
}
|
||
}
|
||
}
|
||
//检查ROI大小
|
||
double obj_L = objROI.right - objROI.left;
|
||
double obj_W = objROI.bottom - objROI.top;
|
||
if (obj_L < obj_W)
|
||
{
|
||
double tmp_value = obj_W;
|
||
obj_W = obj_L;
|
||
obj_L = tmp_value;
|
||
}
|
||
if ((obj_L > algoParam.bagParam.bagL * 0.25) && (obj_W > algoParam.bagParam.bagW * 0.25))
|
||
{
|
||
smallObjPeaks.push_back(residualPeaks[ri]);
|
||
}
|
||
}
|
||
}
|
||
|
||
#endif
|
||
|
||
//目标排序：分层 -> 对最高层分行 -> 对最高层第一行从左到右排序
|
||
if (peakRgns.size() > 0)
|
||
{
|
||
double maxHeight = peakRgns[0].centerPos.z;
|
||
for (int i = 1; i < peakRgns.size(); i++)
|
||
{
|
||
if (maxHeight > peakRgns[i].centerPos.z)
|
||
maxHeight = peakRgns[i].centerPos.z;
|
||
}
|
||
//取同高度层的目标
|
||
std::vector<SSG_peakRgnInfo> level0_objs;
|
||
for (int i = 0, i_max = (int)peakRgns.size(); i < i_max; i++)
|
||
{
|
||
double z_diff = peakRgns[i].centerPos.z - maxHeight;
|
||
if (z_diff < algoParam.bagParam.bagH / 2) //分层
|
||
{
|
||
level0_objs.push_back(peakRgns[i]);
|
||
}
|
||
}
|
||
peakRgns.clear();
|
||
peakRgns.insert(peakRgns.end(), level0_objs.begin(), level0_objs.end());
|
||
int level0_size = (int)peakRgns.size();
|
||
if (level0_size > 1) //进一步排序，分行
|
||
{
|
||
//取Y最小的目标
|
||
double minY = 0;
|
||
double minY_idx = -1;
|
||
for (int i = 0; i < level0_size; i++)
|
||
{
|
||
if (minY_idx < 0)
|
||
{
|
||
minY = peakRgns[i].centerPos.y;
|
||
minY_idx = i;
|
||
}
|
||
else
|
||
{
|
||
if (minY > peakRgns[i].centerPos.y)
|
||
{
|
||
minY = peakRgns[i].centerPos.y;
|
||
minY_idx = i;
|
||
}
|
||
}
|
||
}
|
||
std::vector<int> row_0_outlier;
|
||
for (int i = 0; i < level0_size; i++)
|
||
{
|
||
double y_diff = peakRgns[i].centerPos.y - minY;
|
||
if (y_diff < algoParam.bagParam.bagW / 2) //第一行
|
||
objOps.push_back(peakRgns[i]);
|
||
else
|
||
row_0_outlier.push_back(i); //将其它行的序号记录下来
|
||
}
|
||
//对第一行的目标按从左到右排序
|
||
if (objOps.size() > 1)
|
||
{
|
||
std::sort(objOps.begin(), objOps.end(), compareByXValue);
|
||
}
|
||
for (int i = 0; i < row_0_outlier.size(); i++)
|
||
objOps.push_back(peakRgns[row_0_outlier[i]]);
|
||
#if 0
|
||
for (int i = level0_end + 1; i < peakRgns.size(); i++)
|
||
objOps.push_back(peakRgns[i]);
|
||
#endif
|
||
}
|
||
else
|
||
objOps.insert(objOps.end(), peakRgns.begin(), peakRgns.end());
|
||
}
|
||
//碰撞检测
|
||
if ((objOps.size() > 0) && (smallObjPeaks.size() > 0))
|
||
{
|
||
SSG_peakRgnInfo* highest_obj = &objOps[0];
|
||
double objZ = highest_obj->centerPos.z;
|
||
for (int i = 0; i < smallObjPeaks.size(); i++)
|
||
{
|
||
SSG_2DValueI* a_samllPk = &smallObjPeaks[i];
|
||
if (highest_obj->centerPos.z > a_samllPk->valueD + algoParam.bagParam.bagH / 2)
|
||
{
|
||
SVzNL3DPosition* smallPkPt = &laser3DPoints[a_samllPk->x].p3DPosition[a_samllPk->y];
|
||
double dist = sqrt(pow(highest_obj->centerPos.x - smallPkPt->pt3D.x, 2) + pow(highest_obj->centerPos.y - smallPkPt->pt3D.y, 2));
|
||
double dia_angle = sqrt(pow(highest_obj->objSize.dWidth, 2) + pow(highest_obj->objSize.dHeight, 2));
|
||
//同层比较
|
||
double z_diff = smallPkPt->pt3D.z - objZ;
|
||
if (z_diff < algoParam.bagParam.bagH / 2) //分层
|
||
{
|
||
if (dist < dia_angle / 2)
|
||
objOps.clear(); //本次检测无效
|
||
}
|
||
}
|
||
}
|
||
}
|
||
//将数据重新投射回原来的坐标系，以保持手眼标定结果正确
|
||
for (int i = 0; i < lineNum; i++)
|
||
sg_lineDataR(&laser3DPoints[i], poseCalibPara.invRMatrix, -1);
|
||
//将检测结果重新投射回原来的坐标系
|
||
double invMatrix[3][3];
|
||
invMatrix[0][0] = poseCalibPara.invRMatrix[0];
|
||
invMatrix[0][1] = poseCalibPara.invRMatrix[1];
|
||
invMatrix[0][2] = poseCalibPara.invRMatrix[2];
|
||
invMatrix[1][0] = poseCalibPara.invRMatrix[3];
|
||
invMatrix[1][1] = poseCalibPara.invRMatrix[4];
|
||
invMatrix[1][2] = poseCalibPara.invRMatrix[5];
|
||
invMatrix[2][0] = poseCalibPara.invRMatrix[6];
|
||
invMatrix[2][1] = poseCalibPara.invRMatrix[7];
|
||
invMatrix[2][2] = poseCalibPara.invRMatrix[8];
|
||
for (int i = 0, i_max = (int)objOps.size(); i < i_max; i++)
|
||
{
|
||
SSG_EulerAngles euAngle = { objOps[i].centerPos.x_roll, objOps[i].centerPos.y_pitch, objOps[i].centerPos.z_yaw };
|
||
double pose[3][3];
|
||
eulerToRotationMatrixZYX(euAngle, pose);
|
||
double resultMatrix[3][3];
|
||
for (int i = 0; i < 3; i++)
|
||
{
|
||
for (int j = 0; j < 3; j++)
|
||
{
|
||
resultMatrix[i][j] = 0;
|
||
for (int m = 0; m < 3; m++)
|
||
resultMatrix[i][j] += invMatrix[i][m] * pose[m][j];
|
||
}
|
||
}
|
||
SSG_EulerAngles resultEuAngle = rotationMatrixToEulerZYX(resultMatrix);
|
||
objOps[i].centerPos.z_yaw = resultEuAngle.yaw;
|
||
double x = objOps[i].centerPos.x * poseCalibPara.invRMatrix[0] +
|
||
objOps[i].centerPos.y * poseCalibPara.invRMatrix[1] +
|
||
objOps[i].centerPos.z * poseCalibPara.invRMatrix[2];
|
||
double y = objOps[i].centerPos.x * poseCalibPara.invRMatrix[3] +
|
||
objOps[i].centerPos.y * poseCalibPara.invRMatrix[4] +
|
||
objOps[i].centerPos.z * poseCalibPara.invRMatrix[5];
|
||
double z = objOps[i].centerPos.x * poseCalibPara.invRMatrix[6] +
|
||
objOps[i].centerPos.y * poseCalibPara.invRMatrix[7] +
|
||
objOps[i].centerPos.z * poseCalibPara.invRMatrix[8];
|
||
objOps[i].centerPos.x = x;
|
||
objOps[i].centerPos.y = y;
|
||
objOps[i].centerPos.z = z;
|
||
}
|
||
#endif
|
||
//水平和垂直提取特征
|
||
|
||
//进行距离变换
|
||
//分水岭分割
|
||
//按z高度检测目标
|
||
} |