威海建设局官方网站,选服务好的网站建设,网络建设工程师是干什么的,网站不用域名解绑接上回LeGO-LOAM 几个特有函数的分析#xff08;1#xff09;
二、广度优先遍历
广度优先遍历#xff08;Breadth-First Search, BFS#xff09;是一种用于遍历或搜索树或图的算法。这种算法从树的根#xff08;或图的某一指定节点#xff09;开始#xff0c;然后探索…接上回LeGO-LOAM 几个特有函数的分析1
二、广度优先遍历
广度优先遍历Breadth-First Search, BFS是一种用于遍历或搜索树或图的算法。这种算法从树的根或图的某一指定节点开始然后探索邻近的节点之后对每一个邻近的节点它再去探索它们各自相邻的节点这个过程持续进行直到访问所有可达的节点。
广度优先遍历的主要特点是它按照距离起始点的“层次”来遍历。首先访问距离起点最近的节点然后是它们的邻居如此类推。
2.1 广度优先遍历的步骤 初始化首先将起始节点放入队列中。 遍历 从队列中弹出一个节点。检查该节点是否为目标节点。如果是则完成搜索。将该节点的所有未访问过的邻居节点加入队列。 重复重复步骤2直到队列为空或找到目标节点。 结束当队列为空且目标未找到或已找到目标节点时算法结束。
2.2基于 BFS 的点云聚类和外点剔除
2.2.1原理 2.2.2源码注释 void labelComponents(int row, int col){// use std::queue std::vector std::deque will slow the program down greatly// 声明所需的变量输入的ROW和col是单帧点云第几行第几列的点// 用于存储距离和角度计算的临时变量float d1, d2, alpha, angle;// 用于存储索引的变量int fromIndX, fromIndY, thisIndX, thisIndY;// 标记是否每个扫描线都至少有一个点被添加bool lineCountFlag[N_SCAN] {false};//用两个数组分别保存x,yqueueIndX[0] row;queueIndY[0] col;//算法标志int queueSize 1;// 队列开始的索引int queueStartInd 0;// 队列结束的索引int queueEndInd 1;// 初始化聚类数组allPushedIndX[0] row;allPushedIndY[0] col;//计数int allPushedIndSize 1;//很巧妙有有效邻点就加一每次循环减1实现bfs广度优先遍历关键while(queueSize 0){// Pop point// 取出当前点x,y坐标fromIndX queueIndX[queueStartInd];fromIndY queueIndY[queueStartInd];//队列大小减一--queueSize;//索引加一queueStartInd;// Mark popped point// 标记该点为一类聚类就是给点加标签标签一致的就是一类labelMat.atint(fromIndX, fromIndY) labelCount;// Loop through all the neighboring grids of popped grid// 检查所有邻点for (auto iter neighborIterator.begin(); iter ! neighborIterator.end(); iter){// new index// 计算邻点的索引其实就是上下左右四个点thisIndX fromIndX (*iter).first;thisIndY fromIndY (*iter).second;// index should be within the boundary// 如果raw为0或者15上或者下没有邻点跳过if (thisIndX 0 || thisIndX N_SCAN)continue;// at range image margin (left or right side)//设置矩阵最两边的点也为邻点因为VLP16是360度//在cow为0时左边的邻点在1799if (thisIndY 0)thisIndY Horizon_SCAN - 1;//在cow为1799时左边的邻点在0if (thisIndY Horizon_SCAN)thisIndY 0;// prevent infinite loop (caused by put already examined point back)// 如果该点已被标记则跳过if (labelMat.atint(thisIndX, thisIndY) ! 0)continue;// 计算角度差以决定是否将邻点加入到当前区域// 距离雷达远的是D1近的是D2d1 std::max(rangeMat.atfloat(fromIndX, fromIndY),rangeMat.atfloat(thisIndX, thisIndY));d2 std::min(rangeMat.atfloat(fromIndX, fromIndY), rangeMat.atfloat(thisIndX, thisIndY));//0-101意味着是一条线上的点角度是360/1800*3.14/1800.0035if ((*iter).first 0)alpha segmentAlphaX;else//10-10意味着是上下两条线上的点角度是30/16-1*3.14/1800.035alpha segmentAlphaY;//计算图中angle角度angle atan2(d2*sin(alpha), (d1 -d2*cos(alpha)));//如果角度大于60度if (angle segmentTheta){//把此邻点放入队列queueIndX[queueEndInd] thisIndX;queueIndY[queueEndInd] thisIndY;//增加sizequeueSize;//末尾索引右移queueEndInd;//把此邻点赋上和之前取出来的点一样的标签labelMat.atint(thisIndX, thisIndY) labelCount;//这行有点被标记过lineCountFlag[thisIndX] true;//保存聚类结果allPushedIndX[allPushedIndSize] thisIndX;allPushedIndY[allPushedIndSize] thisIndY;allPushedIndSize;}}}// check if this segment is validbool feasibleSegment false;//如果聚类大于30则认为是一个好的聚类if (allPushedIndSize 30)feasibleSegment true;//如果大于5而且都是竖着的超过3个也认为是一个好聚类可能是树电线杆else if (allPushedIndSize segmentValidPointNum){int lineCount 0;for (size_t i 0; i N_SCAN; i)if (lineCountFlag[i] true)lineCount;if (lineCount segmentValidLineNum)feasibleSegment true; }// segment is valid, mark these points//如果聚类成功标签加一if (feasibleSegment true){labelCount;}else{ // segment is invalid, mark these pointsfor (size_t i 0; i allPushedIndSize; i){//不成功则标记为999999代表依托答辩labelMat.atint(allPushedIndX[i], allPushedIndY[i]) 999999;}}} 需要注意的点
一是 邻点的定义就是代表取当前点上下左右四个点 std::pairint8_t, int8_t neighbor;
neighbor.first -1; neighbor.second 0; neighborIterator.push_back(neighbor);
neighbor.first 0; neighbor.second 1; neighborIterator.push_back(neighbor);
neighbor.first 0; neighbor.second -1; neighborIterator.push_back(neighbor);
neighbor.first 1; neighbor.second 0; neighborIterator.push_back(neighbor);二是 巧妙的通过queueSize 实现广度优先遍历算法的核心
开始是int queueSize 1让其进入循环 while(queueSize 0){//队列大小减一--queueSize;for (auto iter neighborIterator.begin(); iter ! neighborIterator.end(); iter){//如果角度大于60度if (angle segmentTheta){//增加sizequeueSize;}}
} 三是 聚类时候大于30个点或者大于5个点但是有三个竖着的聚为一类
我觉得原因是考虑到竖着的点距离远的因素
四是 通过计算角度来判断是否是邻点
想象一下是不是D1越长angle越小 2.3函数的调用
用此种方式实现了一帧雷达所有点的聚类 for (size_t i 0; i N_SCAN; i)for (size_t j 0; j Horizon_SCAN; j)//上一个函数说过地面点label被置为1 //如果这个点既不是地面点也没有聚类过开始聚类if (labelMat.atint(i,j) 0)labelComponents(i, j);
