559 lines
18 KiB
C++
559 lines
18 KiB
C++
#include <vector>
|
||
#include "SG_baseDataType.h"
|
||
#include "SG_baseAlgo_Export.h"
|
||
#include "WD_particleSizeMeasure_Export.h"
|
||
#include <opencv2/opencv.hpp>
|
||
#include <limits>
|
||
|
||
std::string m_strVersion = "1.0.0";
|
||
const char* wd_particleSegVersion(void)
|
||
{
|
||
return m_strVersion.c_str();
|
||
}
|
||
|
||
//计算一个平面调平参数。
|
||
//数据输入中可以有一个地平面和参考调平平面，以最高的平面进行调平
|
||
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
|
||
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_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.filterParam, 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_INVLD_Q_SCALE;
|
||
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,
|
||
-1.0,
|
||
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::Mat dtImage_color;
|
||
cv::cvtColor(dtImage, dtImage_color, cv::COLOR_GRAY2BGR);
|
||
cv::imwrite("distTransform.png", dtImage_color);
|
||
#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++)
|
||
{
|
||
SSG_2DValueI& obj_0 = filter_dt_peaks[i];
|
||
for (int j = i + 1; j < filterSize; j++)
|
||
{
|
||
SSG_2DValueI& obj_1 = filter_dt_peaks[j];
|
||
double dist = sqrt(pow(obj_0.x - obj_1.x, 2) + pow(obj_0.y - obj_1.y, 2));
|
||
double distTh = dist * 1.2;
|
||
if ((obj_0.valueD + obj_1.valueD) > distTh) //合并
|
||
{
|
||
if (obj_0.valueD < obj_1.valueD)
|
||
obj_0.value = -1;
|
||
else
|
||
obj_1.value = -1;
|
||
}
|
||
}
|
||
}
|
||
|
||
//有效种子
|
||
std::vector<SSG_2DValueI> vld_dt_peaks;
|
||
for (int i = 0; i < filterSize; i++)
|
||
{
|
||
if (filter_dt_peaks[i].value < 0)
|
||
continue;
|
||
vld_dt_peaks.push_back(filter_dt_peaks[i]);
|
||
}
|
||
#if OUTPUT_DEBUG //debug
|
||
int dbg_seedNum = (int)vld_dt_peaks.size();
|
||
for (int i = 0; i < dbg_seedNum; i++)
|
||
{
|
||
int dbg_px = vld_dt_peaks[i].x;
|
||
int dbg_py = vld_dt_peaks[i].y;
|
||
//dtImage_color.at<cv::Vec3b>(dbg_py, dbg_px) = cv::Vec3b(0, 0, 255);
|
||
cv::circle(dtImage_color, cv::Point(dbg_px, dbg_py), 3, cv::Scalar(0, 0, 255), -1);
|
||
}
|
||
cv::imwrite("distTransform_seed.png", dtImage_color);
|
||
#endif
|
||
|
||
//分水岭分割
|
||
//扫描边界，建立相邻目标边界集合
|
||
//通过目标相邻边界判断是否合并相邻目标
|
||
|
||
//获取最大最小值
|
||
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)); // 初始化标记图为1,背景
|
||
int maxValue = (int)maxVal + 2;
|
||
int maxLevel = (int)(maxVal - minVal);
|
||
for (int i = 0; i < distTransform.rows; i++)
|
||
{
|
||
if (i == 758)
|
||
int kkk = 1;
|
||
float* rowPtr = distTransform.ptr<float>(i);
|
||
for (int j = 0; j < distTransform.cols; j++)
|
||
{
|
||
if (j == 171)
|
||
int kkk = 1;
|
||
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;
|
||
}
|
||
}
|
||
}
|
||
int startMarkerID = 2;
|
||
wd_seedWatershed(wsImg, vld_dt_peaks, maxLevel, startMarkerID);
|
||
#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 if (wsImg.markers[i][j] < 2)
|
||
{
|
||
waterShedResult.at<cv::Vec3b>(i, j) = cv::Vec3b(200, 200, 200);
|
||
}
|
||
else
|
||
{ // 区域（根据标记值生成不同颜色）
|
||
|
||
int color_r = (wsImg.markers[i][j] * 97) % 256;
|
||
int color_g = (wsImg.markers[i][j] * 73) % 256;
|
||
int color_b = (wsImg.markers[i][j] * 59) % 256;
|
||
waterShedResult.at<cv::Vec3b>(i, j) = cv::Vec3b(color_b, color_g, color_r);
|
||
}
|
||
}
|
||
}
|
||
cv::imwrite("watershed.png", waterShedResult);
|
||
#endif
|
||
|
||
//生成分割后的目标
|
||
//检查每个3D点在投影上的位置，生成目标的3D点
|
||
std::vector<std::vector< SVzNL3DPoint>> segObjs;
|
||
int maxMkID = startMarkerID + (int)vld_dt_peaks.size();
|
||
segObjs.resize(maxMkID);
|
||
for (int line = 0; line < lineNum; line++)
|
||
{
|
||
for (int i = 0; i < nPointCnt; i++)
|
||
{
|
||
SVzNL3DPosition* pt3D = &scanLines[line][i];
|
||
pt3D->nPointIdx = 0;
|
||
if (pt3D->pt3D.z < 1e-4)
|
||
continue;
|
||
|
||
double x = pt3D->pt3D.x;
|
||
double y = pt3D->pt3D.y;
|
||
int px = (int)(x - x_range.min) / scale + edgeSkip;
|
||
int py = (int)(y - y_range.min) / scale + edgeSkip;
|
||
|
||
int marker = wsImg.markers[py][px];
|
||
if ((marker >= startMarkerID)&&( marker <= maxMkID))
|
||
{
|
||
pt3D->nPointIdx = marker;
|
||
segObjs[marker].push_back(pt3D->pt3D);
|
||
}
|
||
}
|
||
}
|
||
|
||
//生成目标
|
||
for (int i = startMarkerID; i < maxMkID; i++)
|
||
{
|
||
if (segObjs[i].size() < 10)
|
||
continue;
|
||
|
||
//获取投影,并统计最小Z和最大Z
|
||
double minZ = -1;
|
||
double maxZ = 0;
|
||
std::vector<cv::Point2f> points;
|
||
int ptSize = (int)segObjs[i].size();
|
||
for (int m = 0; m < ptSize; m++)
|
||
{
|
||
float x = (float)segObjs[i][m].x;
|
||
float y = (float)segObjs[i][m].y;
|
||
points.push_back(cv::Point2f(x, y));
|
||
if (minZ < 0)
|
||
{
|
||
minZ = segObjs[i][m].z;
|
||
maxZ = segObjs[i][m].z;
|
||
}
|
||
else
|
||
{
|
||
if (minZ > segObjs[i][m].z) minZ = segObjs[i][m].z;
|
||
if (maxZ < segObjs[i][m].z) maxZ = segObjs[i][m].z;
|
||
}
|
||
}
|
||
if (points.size() == 0)
|
||
continue;
|
||
//最小外接矩
|
||
// 最小外接矩形
|
||
cv::RotatedRect rect = minAreaRect(points);
|
||
cv::Point2f vertices[4];
|
||
rect.points(vertices);
|
||
double width = rect.size.width; //投影的宽和高， 对应袋子的长和宽
|
||
double height = rect.size.height;
|
||
if (width < height)
|
||
{
|
||
double tmp = height;
|
||
height = width;
|
||
width = tmp;
|
||
}
|
||
SWD_ParticlePosInfo a_obj;
|
||
a_obj.size.length = width;
|
||
a_obj.size.width = height;
|
||
a_obj.size.height = maxZ - minZ;
|
||
for (int m = 0; m < 4; m++)
|
||
{
|
||
SVzNL3DPoint vPt_btm = { vertices[m].x, vertices[m].y, maxZ };
|
||
SVzNL3DPoint vPt_top = { vertices[m].x, vertices[m].y, minZ };
|
||
a_obj.vertix[m] = vPt_btm;
|
||
a_obj.vertix[m + 4] = vPt_top;
|
||
}
|
||
particles.push_back(a_obj);
|
||
}
|
||
} |