diff --git a/sourceCode/WD_particleSizeMeasure.cpp b/sourceCode/WD_particleSizeMeasure.cpp new file mode 100644 index 0000000..747c5ba --- /dev/null +++ b/sourceCode/WD_particleSizeMeasure.cpp @@ -0,0 +1,1105 @@ +#include +#include "SG_baseDataType.h" +#include "SG_baseAlgo_Export.h" +#include "WD_particleSizeMeasure_Export.h" +#include +#include + +//计算一个平面调平参数。 +//数据输入中可以有一个地平面和参考调平平面，以最高的平面进行调平 +//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数 +SSG_planeCalibPara wd_getBaseCalibPara( + std::vector< std::vector>& 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>& 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> 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 filterData; + std::vector 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>& scanLines, + const SWD_paricleSizeParam particleSizeParam, + const SSG_planeCalibPara groundCalibPara, + const SWD_PSM_algoParam algoParam, + std::vector& 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 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> 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 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 v_feature_trees; + std::vector v_ending_trees; + //特征生长 + wd_getFeatureGrowingTrees_noTypeMatch( + all_vLineFeatures, + v_feature_trees, + v_ending_trees, + algoParam.growParam); + + //水平方向特征生长 + std::vector h_feature_trees; + std::vector 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 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(a_feature->jumpPos2D.y, a_feature->jumpPos2D.x)[0] = treeID; //edgeID + featureMask.at(a_feature->jumpPos2D.y, a_feature->jumpPos2D.x)[1] = a_vEdgeTree->treeType; + featureMask.at(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(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(a_feature->jumpPos2D.x, a_feature->jumpPos2D.y)[0] = treeID; + featureMask.at(a_feature->jumpPos2D.x, a_feature->jumpPos2D.y)[1] += a_hEdgeTree->treeType << 4; + featureMask.at(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(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(a_feature->jumpPos2D.y, a_feature->jumpPos2D.x)[0] = hvTreeIdx; //edgeID + featureMask.at(a_feature->jumpPos2D.y, a_feature->jumpPos2D.x)[1] = a_vTree->treeType; + featureMask.at(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(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(a_feature->jumpPos2D.x, a_feature->jumpPos2D.y)[0] = hvTreeIdx; + featureMask.at(a_feature->jumpPos2D.x, a_feature->jumpPos2D.y)[1] += a_hTree->treeType << 4; + featureMask.at(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(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 dt_peaks; + sg_getLocalPeaks_distTransform(distTransform, dt_peaks, searchWin); + //以大小进行过滤 + double minDistTh = minW * 0.4 / scale; + std::vector 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(wsImg.width)); + wsImg.markers.resize(wsImg.height, std::vector(wsImg.width, 1)); // 初始化标记图为0 + int maxValue = (int)maxVal + 2; + for (int i = 0; i < distTransform.rows; i++) + { + float* rowPtr = distTransform.ptr(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(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(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 dt_peaks; + sg_getLocalPeaks_distTransform(distTransform, dt_peaks, searchWin); + //获取Peaks + int invlidDistToEdge = 50; //距离边缘近的Peak是非法点。 + double minPeakValue = algoParam.bagParam.bagW / 8; + std::vector 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(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 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 matchTable_TB; + std::vector< SSG_matchPair> TB_pairs; + std::vector 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 matchTable_LR; + std::vector 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 iter_objs; + //将没有处理的Peak点保留 + std::vector 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 matchTable_TB; + std::vector< SSG_matchPair> TB_pairs; + std::vector 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 matchTable_LR; + std::vector 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 smallObjPeaks; //记录0.25L * 0.25W的目标，用于碰撞检查 + //将最后没有处理的Peak点保留 + std::vector 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& 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& 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& 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& 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 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 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高度检测目标 +} \ No newline at end of file diff --git a/sourceCode/WD_particleSizeMeasure_Export.h b/sourceCode/WD_particleSizeMeasure_Export.h new file mode 100644 index 0000000..133ddda --- /dev/null +++ b/sourceCode/WD_particleSizeMeasure_Export.h @@ -0,0 +1,54 @@ +#pragma once + +#if defined(SG_API_LIBRARY) +# define SG_APISHARED_EXPORT __declspec(dllexport) +#else +# define SG_APISHARED_EXPORT __declspec(dllimport) +#endif + +#include "SG_baseDataType.h" +#include +#include + +#define OUTPUT_DEBUG 1 + +typedef struct +{ + SWD_sizeParam minSize; + SWD_sizeParam alarmSize; +}SWD_paricleSizeParam; + +typedef struct +{ + SSG_outlierFilterParam filterParam; + SSG_cornerParam cornerParam; + SSG_treeGrowParam growParam; +}SWD_PSM_algoParam; + +typedef struct +{ + double EQRadius; + SVzNL3DPoint center_pos; +}SWD_ParticlePosInfo; + +//计算一个平面调平参数。 +//数据输入中可以有一个地平面和参考调平平面，以最高的平面进行调平 +//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数 +SG_APISHARED_EXPORT SSG_planeCalibPara wd_getBaseCalibPara( + std::vector< std::vector>& scanLines); + +//相机姿态调平，并去除地面 +SG_APISHARED_EXPORT void wd_lineDataR( + std::vector< SVzNL3DPosition>& a_line, + const double* camPoseR, + double groundH); + +//粒径检测 +SG_APISHARED_EXPORT void wd_particleSizeMeasure( + std::vector< std::vector>& scanLines, + const SWD_paricleSizeParam particleSizeParam, + const SSG_planeCalibPara groundCalibPara, + const SWD_PSM_algoParam algoParam, + std::vector& particles, + int* errCode); + diff --git a/sourceCode/WD_watershed.cpp b/sourceCode/WD_watershed.cpp new file mode 100644 index 0000000..a718f89 --- /dev/null +++ b/sourceCode/WD_watershed.cpp @@ -0,0 +1,366 @@ + +#if 0 +#include +#include +#include +#include +#include +#include + +using namespace std; + +// 图像尺寸 +const int WIDTH = 256; +const int HEIGHT = 256; + +// 8邻域方向 +const int dx[] = { -1, -1, -1, 0, 0, 1, 1, 1 }; +const int dy[] = { -1, 0, 1, -1, 1, -1, 0, 1 }; + +// 像素结构 +struct Pixel { + int x, y; // 坐标 + int value; // 像素值(灰度) + Pixel(int x_, int y_, int v_) : x(x_), y(y_), value(v_) {} + // 优先队列需要的比较函数(小顶堆) + bool operator<(const Pixel& other) const { + return value > other.value; // 注意：优先队列默认是大顶堆，这里反转比较 + } +}; + +// 读取PGM格式图像 +bool readPGM(const string& filename, int image[HEIGHT][WIDTH]) { + ifstream file(filename, ios::binary); + if (!file) return false; + + string magic; + int w, h, max_val; + file >> magic >> w >> h >> max_val; + if (magic != "P5" || w != WIDTH || h != HEIGHT) return false; + + // 跳过换行符 + file.ignore(1); + + // 读取像素值 + unsigned char pixel; + for (int i = 0; i < HEIGHT; ++i) { + for (int j = 0; j < WIDTH; ++j) { + file.read((char*)&pixel, 1); + image[i][j] = (int)pixel; + } + } + return true; +} + +// 保存分割结果为PGM图像 +void writePGM(const string& filename, int labels[HEIGHT][WIDTH], int num_labels) { + ofstream file(filename, ios::binary); + file << "P5\n" << WIDTH << " " << HEIGHT << "\n255\n"; + + // 将标签映射到0-255范围 + int step = 255 / (num_labels + 1); + for (int i = 0; i < HEIGHT; ++i) { + for (int j = 0; j < WIDTH; ++j) { + unsigned char val = (labels[i][j] + 1) * step; + if (labels[i][j] == -1) val = 0; // 分水岭边界 + file.write((char*)&val, 1); + } + } +} + +// 分水岭算法实现 +int watershed(int image[HEIGHT][WIDTH], int labels[HEIGHT][WIDTH]) { + // 初始化标签：-1表示未标记，0表示边界 + memset(labels, -1, sizeof(int) * HEIGHT * WIDTH); + + // 优先队列(按像素值从小到大处理) + priority_queue pq; + + // 标记所有局部最小值作为初始盆地 + int current_label = 0; + bool is_min; + + // 第一步：找到所有局部最小值并标记 + for (int i = 0; i < HEIGHT; ++i) { + for (int j = 0; j < WIDTH; ++j) { + is_min = true; + for (int d = 0; d < 8; ++d) { + int ni = i + dx[d]; + int nj = j + dy[d]; + if (ni >= 0 && ni < HEIGHT && nj >= 0 && nj < WIDTH) { + if (image[ni][nj] < image[i][j]) { + is_min = false; + break; + } + } + } + if (is_min) { + labels[i][j] = current_label++; + pq.push(Pixel(i, j, image[i][j])); + } + } + } + + // 第二步：模拟淹没过程 + while (!pq.empty()) { + Pixel p = pq.top(); + pq.pop(); + int x = p.x; + int y = p.y; + + for (int d = 0; d < 8; ++d) { + int nx = x + dx[d]; + int ny = y + dy[d]; + + if (nx >= 0 && nx < HEIGHT && ny >= 0 && ny < WIDTH) { + // 如果邻域像素未标记 + if (labels[nx][ny] == -1) { + // 标记为当前区域 + labels[nx][ny] = labels[x][y]; + pq.push(Pixel(nx, ny, image[nx][ny])); + } + // 如果邻域像素已标记且属于不同区域，则标记为边界 + else if (labels[nx][ny] != labels[x][y]) { + labels[x][y] = -1; // 当前像素设为边界 + } + } + } + } + + return current_label; +} + +#if 0 +算法说明 +这个实现遵循分水岭算法的基本原理： + +地形表示：将图像的灰度值视为地形高度，灰度值越低表示地势越低 +初始盆地：首先识别图像中的所有局部最小值，这些是初始的 "积水盆地" +淹没过程：使用优先队列（最小堆）模拟从低到高的淹没过程，按像素值从小到大处理 +区域生长：每个像素被分配给其相邻的最低盆地 +边界确定：当两个不同盆地的水相遇时，形成分水岭边界 + +使用方法 + +准备一张 256x256 的 PGM 格式灰度图像，命名为input.pgm +编译代码：g++ watershed.cpp - o watershed +运行程序：. / watershed +结果将保存为output.pgm，其中不同区域用不同灰度表示，边界为黑色 + +注意事项 + +这个实现是基础版本，没有加入预处理步骤（如去噪），可能对复杂图像分割效果不佳 +实际应用中通常需要先对图像进行平滑处理，减少过度分割 +可以通过调整邻域处理方式（4 邻域或 8 邻域）来改变分割效果 +对于彩色图像，需要先转换为灰度图再处理 + +如果需要处理更大尺寸的图像，可以修改WIDTH和HEIGHT常量，并相应调整内存分配 +int main() { + int image[HEIGHT][WIDTH]; + int labels[HEIGHT][WIDTH]; + + // 读取输入图像 + if (!readPGM("input.pgm", image)) { + cerr << "无法读取图像文件！" << endl; + return -1; + } + + // 执行分水岭算法 + int num_labels = watershed(image, labels); + cout << "分割完成，共得到 " << num_labels << " 个区域" << endl; + + // 保存结果 + writePGM("output.pgm", labels, num_labels); + cout << "结果已保存为 output.pgm" << endl; + + return 0; +} +#endif + +#else +#include +#include +#include +#include +#include +#include +#include "SG_baseDataType.h" +#include "SG_baseAlgo_Export.h" +using namespace std; + +#if 0 +// 读取PPM格式图像（简化版） +bool readPPM(const string& filename, Image& img) { + ifstream file(filename, ios::binary); + if (!file) return false; + + string magic; + int maxVal; + file >> magic >> img.width >> img.height >> maxVal; + if (magic != "P6") return false; // 仅支持二进制PPM + + file.ignore(); // 忽略换行符 + vector data(img.width * img.height * 3); + file.read((char*)data.data(), data.size()); + + // 转为灰度图（Y = 0.299*R + 0.587*G + 0.114*B） + img.gray.resize(img.height, vector(img.width)); + img.markers.resize(img.height, vector(img.width, 0)); // 初始化标记图为0 + + for (int i = 0; i < img.height; ++i) { + for (int j = 0; j < img.width; ++j) { + int idx = (i * img.width + j) * 3; + int r = data[idx]; + int g = data[idx + 1]; + int b = data[idx + 2]; + img.gray[i][j] = (int)(0.299 * r + 0.587 * g + 0.114 * b); + } + } + return true; +} + +// 保存分割结果为PPM格式 +void saveResult(const string& filename, const Image& img) { + ofstream file(filename, ios::binary); + file << "P6\n" << img.width << " " << img.height << "\n255\n"; + + for (int i = 0; i < img.height; ++i) { + for (int j = 0; j < img.width; ++j) { + if (img.markers[i][j] == -1) { // 分水岭边界（红色） + file.put(255); file.put(0); file.put(0); + } + else { // 区域（根据标记值生成不同颜色） + int color = (img.markers[i][j] * 50) % 256; + file.put(color); + file.put((color + 85) % 256); + file.put((color + 170) % 256); + } + } + } +} +#endif + + + +// 8邻域坐标偏移 +const int dx[] = { -1, -1, -1, 0, 0, 1, 1, 1 }; +const int dy[] = { -1, 0, 1, -1, 1, -1, 0, 1 }; + +// 计算局部极小值作为初始种子点 +void findMinima(SWD_waterShedImage& img, int& markerCount) { + markerCount = 1; //背景对应的ID + // 遍历每个像素，判断是否为局部极小值（8邻域内最小） + for (int i = 1; i < img.height - 1; ++i) { + for (int j = 1; j < img.width - 1; ++j) { + if (1 == img.markers[i][j]) //背景 + continue; + bool isMin = true; +#if 0 + for (int d = 0; d < 8; ++d) { + int y = i + dy[d]; + int x = j + dx[d]; + if (img.gray[y][x] < img.gray[i][j]) { + isMin = false; + break; + } + } +#else + for (int dy = -7; dy < 7; dy++) + { + for (int dx = -7; dx < 7; dx++) + { + int y = i + dy; + int x = j + dx; + if ((x >= 0) && (x < img.width) && (y >= 0) && (y < img.height)) + { + if (img.gray[y][x] < img.gray[i][j]) { + isMin = false; + break; + } + } + } + } +#endif + if (isMin) { + // 避免重复标记同一区域的极小值 + bool hasMarker = false; + for (int d = 0; d < 8; ++d) { + int y = i + dy[d]; + int x = j + dx[d]; + if (img.markers[y][x] > 1) { + hasMarker = true; + img.markers[y][x] = img.markers[i][j]; + break; + } + } + if (!hasMarker) { + img.markers[i][j] = ++markerCount; // 新种子点 + } + } + } + } +} + +// 分水岭算法主函数 +void watershed(SWD_waterShedImage& img) +{ + int markerCount; + findMinima(img, markerCount); // 自动标记种子点 + + // 按灰度值排序所有像素（模拟水位上升） + vector>> pixels; // (灰度值, (x,y)) + for (int i = 0; i < img.height; ++i) { + for (int j = 0; j < img.width; ++j) { + pixels.emplace_back(img.gray[i][j], make_pair(i, j)); + } + } + sort(pixels.begin(), pixels.end()); + + // 遍历排序后的像素，执行注水过程 + for (const auto& p : pixels) { + int x = p.second.first; + int y = p.second.second; + if (img.markers[x][y] > 0) continue; // 已标记的种子点 + + // 收集邻域已标记的区域 + vector neighborMarkers; + for (int d = 0; d < 8; ++d) { + int nx = x + dx[d]; + int ny = y + dy[d]; + if (nx >= 0 && nx < img.height && ny >= 0 && ny < img.width) { + int m = img.markers[nx][ny]; + if (m > 0) { + neighborMarkers.push_back(m); + } + } + } + + if (neighborMarkers.empty()) { + continue; // 无邻域标记，暂不处理 + } + else if (neighborMarkers.size() == 1) { + img.markers[x][y] = neighborMarkers[0]; // 属于同一区域 + } + else { + // 多区域交汇，标记为分水岭 + img.markers[x][y] = -1; + } + } +} + +#if 0 +int main() { + Image img; + if (!readPPM("input.ppm", img)) { // 输入PPM图像 + cerr << "无法读取图像！" << endl; + return -1; + } + + watershed(img); // 执行分水岭分割 + saveResult("watershed_result.ppm", img); // 保存结果 + + cout << "分割完成，结果已保存为 watershed_result.ppm" << endl; + return 0; +} +#endif +#endif \ No newline at end of file